// Copyright 2020 Makani Technologies LLC
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <gtest/gtest.h>

#include <math.h>

#include "sim/math/linalg.h"

// The remaining tests are generated code comparing the results to
// MATLAB.

TEST(GSLMatTraceTest, Normal0) {
  double m_data[9] = {
      0.4544003592855579,  0.4541216144867326,  0.7570697687169641,
      -0.4220290290438373, -0.5811211069795446, 0.7146712786902283,
      0.6991766186347612,  0.2606303531880529,  0.9839916079109012};
  gsl_block m_block = {9, m_data};
  const gsl_matrix m = {3, 3, 3, m_block.data, &m_block, 0};
  double m_trace = gsl_matrix_trace(&m);

  EXPECT_NEAR(0.8572708602169146, m_trace, 1e-9);
}

TEST(GSLMatDiagTest, Normal0) {
  double m_data[484] = {
      -0.4830996972170225, 0.6916068001931850,  0.5653648337140147,
      -0.5127920011272906, 0.9195757878627335,  -0.4459327109112239,
      0.7919650295736314,  0.1203111673793582,  -0.4743584694439478,
      -0.5439095259960196, -0.6819475471426546, -0.2878639660234925,
      0.5325588580746901,  0.2728879621897584,  -0.7637691459095506,
      -0.9545210505756778, 0.4027026490639480,  -0.0972987801941649,
      -0.4968116636297848, -0.4107728450560240, -0.4754992363289197,
      -0.8137443796044186, -0.9231553602812748, 0.4589259711470723,
      0.2314725479683408,  0.6984107506394348,  0.8028451634961025,
      -0.6641621523381429, -0.5489419844219976, -0.9120320316433728,
      0.3318048894361909,  0.2290682746703283,  -0.9939703339403183,
      0.1779393244468639,  -0.1960861174028070, 0.5004625031449950,
      0.2236956624851079,  -0.7471780138442117, -0.6359073264089210,
      0.8902697283846168,  0.3229642692546391,  -0.2806834929593409,
      -0.5656247345648993, 0.3884737476669939,  -0.2278026720360145,
      0.5418188329099831,  0.2004573385825896,  -0.6687971816684477,
      0.6599076801606711,  0.1712432676141122,  0.0451612961133447,
      0.1650291761844860,  0.4725067981394904,  -0.6545717649187126,
      -0.0391720001996532, -0.7188873304731238, -0.9259146233835942,
      0.5505625610897258,  -0.0402487808158665, 0.9442516692949543,
      0.4414842346146832,  -0.0511144231293001, -0.5424168543576759,
      0.7726444811392215,  0.1446651325696462,  -0.6049795684707804,
      0.3121464586449545,  0.9178754496307771,  -0.1446997933300362,
      0.0130227044755817,  0.3503162626705241,  0.2674433189118164,
      -0.2061715785301237, -0.5254030142325650, 0.7386248941567850,
      -0.4612232768470739, -0.8708981858347595, -0.0358639886632208,
      -0.4066054276846349, 0.7338783342702269,  0.7991492955325215,
      -0.6640431543158589, 0.2132570119201320,  -0.0648884443683231,
      -0.7548076571565581, -0.6521704422307164, -0.4338183776619027,
      0.9233962692557960,  0.5811859629015408,  0.0413039433257278,
      0.1601897034955511,  0.8264218437770381,  0.6140936231935474,
      0.7857832103283664,  0.8606211659267344,  -0.9757976033633597,
      0.2815870121325075,  0.3312494021916321,  0.8678778096773361,
      0.8791794038033034,  -0.7077277301972762, 0.1854299166582933,
      -0.9918549552634650, 0.2075756491468896,  -0.8648648959976213,
      0.5317170488634311,  -0.9950621386298837, -0.1905694599542231,
      -0.5431218260845314, -0.5031016280192397, -0.2007402933214233,
      0.0205208349033210,  -0.1438686871495198, 0.8420114420131575,
      0.2605656957992768,  -0.9309107927791354, 0.4984800705693784,
      -0.5981354746612575, 0.8168715991192053,  -0.6043735903539009,
      0.6280583587317357,  -0.8586908723577751, -0.2152969831348854,
      -0.9198447640128118, 0.3764621377382822,  -0.2112405891565678,
      -0.5073893320627969, 0.3774032052262990,  0.3785951413341218,
      0.4475848343933917,  -0.3483311323192728, 0.4627362638833896,
      0.4446003518376411,  0.4509901260680738,  0.3426734946299088,
      -0.6763637083427534, 0.4856778791661713,  0.7982576750757842,
      0.5270289236303241,  -0.4447866123370150, -0.8878086667935976,
      0.9992699715199445,  -0.1093785589138361, -0.5625840396978665,
      -0.9952171334512352, 0.5094601849922462,  -0.4659603145023175,
      0.4392355741608911,  -0.7103116935257796, 0.9271611152837100,
      0.2887634520126996,  0.6584196828341082,  0.4579412539930312,
      0.0498875952465365,  -0.9271500639711621, -0.7249455547194936,
      -0.5334189101655196, -0.0863678034960516, -0.3567832603321217,
      -0.3323133234393580, 0.2673143832374629,  -0.6334737913667017,
      0.8102745436254366,  0.3044902418229469,  0.0337159800109625,
      -0.7627928889965208, 0.5729831649572130,  -0.0854957209943779,
      0.5537595518069636,  0.3341108251099092,  -0.1683101285905484,
      0.9968302761657899,  -0.1936547255447369, -0.9864196746828660,
      -0.9594609519893431, 0.1564054897849587,  -0.0598214738954255,
      -0.1151833118969112, 0.9913539847317210,  -0.8023639014165720,
      0.5566181827999006,  0.4954047249089635,  0.5548888677474180,
      0.6769858352543601,  0.1090763597812796,  0.9408589116116350,
      -0.4361172019732835, 0.2746002571478992,  0.0344899558009164,
      0.6888425420984552,  -0.8839112242757425, -0.9301211120990667,
      0.5884593538655045,  0.9437541093407011,  0.1401173909637214,
      -0.8954641566972439, -0.7191725347761235, 0.9404252838711387,
      0.9061217491173328,  -0.9490748614833537, 0.1490047721834795,
      -0.1644879149932854, 0.0839612311831071,  -0.5062570639231658,
      -0.2117005486838590, 0.8927044101685673,  -0.4608026922666908,
      0.9436444865162106,  0.9115443323341281,  -0.9632122590173644,
      -0.6569821666679230, 0.2611970564707073,  -0.6189511648907760,
      -0.9409683380691614, 0.2067148403953118,  -0.7655993117142299,
      -0.9979763064358147, 0.8971949027951096,  0.4376754233693294,
      -0.3895440680827962, 0.8104901193982330,  0.4843592617604149,
      -0.3474220477191863, 0.6427201538828351,  0.1145621134624220,
      0.3430758576211665,  -0.2188018092068686, -0.3081543086153329,
      0.4463403261173531,  0.6591413121854388,  -0.0939198211449610,
      0.6066094436899383,  0.8529021870114979,  -0.0408559808777271,
      0.0081440728617221,  0.8631247814912648,  -0.8158460071292502,
      -0.9734457758547381, 0.0129135603023602,  0.0612471288695586,
      0.1954418421389532,  0.8579482121325355,  -0.2607395966399593,
      -0.1587031961706715, -0.9547117549742801, -0.9340521997382087,
      -0.6951257011121486, -0.2031135335265573, -0.4019846845889379,
      0.5194349701509868,  -0.0928760922508911, 0.7552187320101460,
      0.3217417403784191,  -0.9349623812946675, 0.4160160697753239,
      -0.1776006413724596, 0.8826932072969620,  0.9986992415066285,
      -0.4786452541648969, 0.3420691653287908,  0.9892006454553519,
      -0.6327108379186486, 0.8430791939976203,  0.0105756207077567,
      0.6686579438575668,  -0.3384193231294899, -0.0675251687898835,
      0.5517522819833633,  -0.4643466868093897, 0.6224819598251061,
      -0.9445145506261914, 0.1616014073734819,  0.8029138081386473,
      -0.7139279817043247, -0.6890010996483866, 0.8801811548637448,
      -0.2083752660290958, -0.2043299090933990, -0.1576268141660597,
      -0.4011196692210388, 0.5294211735511232,  0.1852080367028233,
      0.8710460692622675,  0.3274679673621781,  0.4559087118305252,
      -0.1065749482257137, -0.4214636220747352, -0.0791940929438708,
      0.5367702526124893,  0.2388025183107527,  0.7954443333909311,
      -0.2799285848080595, -0.2196442368237426, 0.5597497128325184,
      0.4006302329035161,  0.4245318821730140,  -0.8910973380600846,
      0.4951709919787415,  -0.5824329527967755, -0.2836530065176404,
      0.0597372033265351,  -0.2790027034102058, -0.8474797434833554,
      0.7789580087308183,  -0.3636782126425873, -0.4790044566283911,
      0.9460879068575427,  0.2440477450748526,  0.0901187511011177,
      0.4092843827945352,  -0.4260820531771103, -0.7807407593672366,
      0.6766932755174186,  -0.7336872774962144, 0.9126289942107253,
      0.8364899645947885,  -0.6274544547326830, -0.0996508706938897,
      0.8522384642815815,  0.5266401645343213,  -0.4510204716959005,
      0.3205574523156975,  -0.8618765876191983, -0.0322655344215750,
      0.2301034904467640,  0.6640159300455803,  -0.7829372380496851,
      0.8711808944122919,  0.3077012274666735,  -0.0137365018264168,
      0.2103952313755282,  -0.2678061115585173, 0.0239257162803643,
      -0.1478831571559869, -0.9962267335320423, -0.7546510030033908,
      0.3503899871409848,  -0.1718098085279531, -0.8865267134926922,
      -0.0846594657701121, -0.2882121460883529, 0.7742437887182991,
      0.1083629892868154,  0.8833844404841125,  0.1022774125584802,
      0.5072497428336891,  0.6944658355703914,  0.7029988186495963,
      -0.5765657162411022, 0.1043461863388142,  -0.1073352500898010,
      -0.1551049713745651, -0.1409219919528146, -0.7358553512643784,
      -0.2303068939695418, -0.2567374627753023, -0.2110364564189453,
      0.3704451278835053,  0.7621759362910447,  -0.7956170810462897,
      0.2335184186202997,  -0.7238590806144332, 0.5221812185011097,
      0.6623572014447869,  -0.2094894230836941, 0.1495163130482855,
      0.5801197924928927,  0.9555989925727844,  -0.2011254065554515,
      0.0394577707625083,  -0.8891083308891483, -0.5023288428880606,
      0.9483367200791142,  0.4257691946653870,  0.3516587125870898,
      -0.7715167570048538, 0.4617951054964475,  -0.0123440884023902,
      0.4697670300417833,  0.6640463277605138,  0.3283086149111243,
      -0.5233574216552079, 0.1484516472371196,  0.4429377492576236,
      -0.1863803112186877, -0.4343764943001593, -0.2308901701131918,
      0.2187788454503548,  -0.8245191907506253, -0.9224132139416199,
      0.3221409527566221,  -0.3729101181325767, 0.2819795133382927,
      -0.6935306236627714, 0.3188191594298864,  0.7698135233274099,
      0.9983854809458934,  0.3949570576166892,  -0.4544887764605945,
      -0.9739330432167523, -0.8089695215392609, 0.3433385306612882,
      0.3823580496403809,  -0.7565850357652164, 0.4859060333147978,
      0.3163411417040598,  0.2697277144474661,  0.2793196614546709,
      0.9077912846400851,  -0.2152780573264821, -0.1740965642562382,
      0.8056241111777676,  -0.3562325316831352, -0.8922485430597333,
      -0.3357217629614622, -0.0452877914192231, 0.8291068808698860,
      -0.4067196052645288, -0.6479190339275105, -0.0983151348130065,
      -0.7796112982447443, 0.4058302368576159,  -0.3245848530679498,
      0.2126343590682989,  -0.0468320656530856, 0.6959207052249825,
      -0.6788385310100427, 0.7757915017601602,  -0.9901440540999364,
      0.6226435314623364,  0.8193497634327238,  0.8219197716963402,
      -0.9664022577437645, 0.3229926651174486,  0.2605372714951533,
      -0.1261440477220708, -0.2326318436530428, 0.6101651740785661,
      -0.0057481793823870, -0.2206420904735540, 0.5350987369498024,
      -0.1104057043353737, 0.9265032594833635,  0.1015556273908707,
      0.5257322073508937,  -0.7918223874050583, -0.9488568489780205,
      -0.2930927517057720, 0.1465990779406992,  -0.6921443256757072,
      0.2242509535161292,  -0.4670684015922699, 0.7946423012944603,
      0.2852868684863044,  -0.1081035450393890, -0.9437773616197127,
      -0.8660262751930830, -0.3882684191047061, 0.0713835815746082,
      0.3978295151534732,  -0.4567724884935798, -0.8077275914120787,
      0.3974766927161388,  -0.5062591752055867, 0.5911502601488472,
      -0.4500020725584011, 0.7635724211233310,  -0.6046805279105418,
      -0.0050493976571273, -0.0126996282036920, -0.2064194214716288,
      -0.1039348835150524, 0.5792429032034745,  -0.0230995485095804,
      0.2317475849164190,  -0.3210460121692242, 0.3598508278907722,
      0.1835346971592524,  -0.7516847228333041, -0.2294724120759148,
      -0.2943664462487412, 0.1386417737667867,  -0.2825645933879546,
      0.1620062141959595};
  gsl_block m_block = {484, m_data};
  const gsl_matrix m = {22, 22, 22, m_block.data, &m_block, 0};
  double v_out_data[22] = {
      -0.4830996972170225, 0.4589259711470723,  0.2004573385825896,
      0.0130227044755817,  0.6140936231935474,  -0.9309107927791354,
      0.5270289236303241,  -0.6334737913667017, 0.1090763597812796,
      0.9436444865162106,  -0.0939198211449610, -0.9349623812946675,
      -0.2083752660290958, -0.2836530065176404, -0.8618765876191983,
      0.5072497428336891,  -0.2011254065554515, -0.3729101181325767,
      -0.3357217629614622, 0.6101651740785661,  -0.4567724884935798,
      0.1620062141959595};
  gsl_block v_out_block = {22, v_out_data};
  gsl_vector v_out = {22, 1, v_out_block.data, &v_out_block, 0};
  gsl_matrix_diag(&m, &v_out);

  EXPECT_NEAR(-0.4830996972170225, v_out.data[0], 1e-9);
  EXPECT_NEAR(0.4589259711470723, v_out.data[1], 1e-9);
  EXPECT_NEAR(0.2004573385825896, v_out.data[2], 1e-9);
  EXPECT_NEAR(0.0130227044755817, v_out.data[3], 1e-9);
  EXPECT_NEAR(0.6140936231935474, v_out.data[4], 1e-9);
  EXPECT_NEAR(-0.9309107927791354, v_out.data[5], 1e-9);
  EXPECT_NEAR(0.5270289236303241, v_out.data[6], 1e-9);
  EXPECT_NEAR(-0.6334737913667017, v_out.data[7], 1e-9);
  EXPECT_NEAR(0.1090763597812796, v_out.data[8], 1e-9);
  EXPECT_NEAR(0.9436444865162106, v_out.data[9], 1e-9);
  EXPECT_NEAR(-0.0939198211449610, v_out.data[10], 1e-9);
  EXPECT_NEAR(-0.9349623812946675, v_out.data[11], 1e-9);
  EXPECT_NEAR(-0.2083752660290958, v_out.data[12], 1e-9);
  EXPECT_NEAR(-0.2836530065176404, v_out.data[13], 1e-9);
  EXPECT_NEAR(-0.8618765876191983, v_out.data[14], 1e-9);
  EXPECT_NEAR(0.5072497428336891, v_out.data[15], 1e-9);
  EXPECT_NEAR(-0.2011254065554515, v_out.data[16], 1e-9);
  EXPECT_NEAR(-0.3729101181325767, v_out.data[17], 1e-9);
  EXPECT_NEAR(-0.3357217629614622, v_out.data[18], 1e-9);
  EXPECT_NEAR(0.6101651740785661, v_out.data[19], 1e-9);
  EXPECT_NEAR(-0.4567724884935798, v_out.data[20], 1e-9);
  EXPECT_NEAR(0.1620062141959595, v_out.data[21], 1e-9);
}

TEST(GSLVecSaturateTest, Bounded0) {
  double v0_data[22] = {
      -0.5630180192038090, 0.7249701861229663,  -0.6335878247323636,
      0.0018424652756888,  0.7257358960810825,  -0.5638349827145632,
      -0.3885038144135886, -0.5369261711721744, 0.7184807687090926,
      0.1132638024555173,  -0.2505778936114673, 0.8116559405626289,
      -0.2465645137777626, -0.4866282743253405, -0.8151198832567284,
      -0.2592603334760499, 0.3567131447772489,  -0.8469412895140154,
      -0.3357265872788724, 0.1067358008325698,  -0.9722248317342366,
      -0.2842082715660674};
  gsl_block v0_block = {22, v0_data};
  const gsl_vector v0 = {22, 1, v0_block.data, &v0_block, 0};
  double v_out_data[22] = {
      -0.3276431363464143, 0.7249701861229663,  -0.3276431363464143,
      0.0018424652756888,  0.7257358960810825,  -0.3276431363464143,
      -0.3276431363464143, -0.3276431363464143, 0.7184807687090926,
      0.1132638024555173,  -0.2505778936114673, 0.7470493084453584,
      -0.2465645137777626, -0.3276431363464143, -0.3276431363464143,
      -0.2592603334760499, 0.3567131447772489,  -0.3276431363464143,
      -0.3276431363464143, 0.1067358008325698,  -0.3276431363464143,
      -0.2842082715660674};
  gsl_block v_out_block = {22, v_out_data};
  gsl_vector v_out = {22, 1, v_out_block.data, &v_out_block, 0};
  gsl_vector_saturate(&v0, -0.3276431363464143, 0.7470493084453584, &v_out);

  EXPECT_NEAR(-0.3276431363464143, v_out.data[0], 1e-9);
  EXPECT_NEAR(0.7249701861229663, v_out.data[1], 1e-9);
  EXPECT_NEAR(-0.3276431363464143, v_out.data[2], 1e-9);
  EXPECT_NEAR(0.0018424652756888, v_out.data[3], 1e-9);
  EXPECT_NEAR(0.7257358960810825, v_out.data[4], 1e-9);
  EXPECT_NEAR(-0.3276431363464143, v_out.data[5], 1e-9);
  EXPECT_NEAR(-0.3276431363464143, v_out.data[6], 1e-9);
  EXPECT_NEAR(-0.3276431363464143, v_out.data[7], 1e-9);
  EXPECT_NEAR(0.7184807687090926, v_out.data[8], 1e-9);
  EXPECT_NEAR(0.1132638024555173, v_out.data[9], 1e-9);
  EXPECT_NEAR(-0.2505778936114673, v_out.data[10], 1e-9);
  EXPECT_NEAR(0.7470493084453584, v_out.data[11], 1e-9);
  EXPECT_NEAR(-0.2465645137777626, v_out.data[12], 1e-9);
  EXPECT_NEAR(-0.3276431363464143, v_out.data[13], 1e-9);
  EXPECT_NEAR(-0.3276431363464143, v_out.data[14], 1e-9);
  EXPECT_NEAR(-0.2592603334760499, v_out.data[15], 1e-9);
  EXPECT_NEAR(0.3567131447772489, v_out.data[16], 1e-9);
  EXPECT_NEAR(-0.3276431363464143, v_out.data[17], 1e-9);
  EXPECT_NEAR(-0.3276431363464143, v_out.data[18], 1e-9);
  EXPECT_NEAR(0.1067358008325698, v_out.data[19], 1e-9);
  EXPECT_NEAR(-0.3276431363464143, v_out.data[20], 1e-9);
  EXPECT_NEAR(-0.2842082715660674, v_out.data[21], 1e-9);
}

TEST(GSLVecNormBoundTest, Normal0) {
  double v0_data[22] = {
      -0.5233505840631365, 0.3205924951655204,  -0.1764157066097130,
      -0.4418238201905387, -0.7552295613617694, 0.4698437118818046,
      -0.4862090536386232, -0.5134621890701097, -0.0747205857929809,
      0.8614516030998667,  -0.9445012677093043, 0.9178178682454752,
      0.3677111099637114,  -0.7275077191271311, 0.8944768229490447,
      0.5349712728567793,  -0.5068430166014533, 0.1391492690605933,
      0.9488655634746668,  -0.3609853836625756, 0.1620396791059344,
      -0.1152666803726785};
  gsl_block v0_block = {22, v0_data};
  const gsl_vector v0 = {22, 1, v0_block.data, &v0_block, 0};
  double v_norm = gsl_vector_norm_bound(&v0, 0.0767251295010443);

  EXPECT_NEAR(2.7337779494875329, v_norm, 1e-9);
}

TEST(GSLVecNormBoundTest, Bounded0) {
  double v0_data[22] = {
      -0.4271082532455386, -0.4932472312396556, -0.8381156002302570,
      0.5068401220422574,  0.8101771515938763,  0.6177786412233512,
      -0.0948724548931887, -0.9321848017491436, 0.3013599966126816,
      -0.6377772127536068, -0.8594673130425536, -0.8644202429157575,
      -0.5215039440271954, 0.2320621638656115,  -0.1842996997808488,
      -0.8703581692201734, 0.0113425003199297,  0.0810763952602296,
      -0.6119688737608362, 0.4196784976060979,  -0.0026852403894346,
      0.2389966090583828};
  gsl_block v0_block = {22, v0_data};
  const gsl_vector v0 = {22, 1, v0_block.data, &v0_block, 0};
  double v_norm = gsl_vector_norm_bound(&v0, 2.4050296055852645);

  EXPECT_NEAR(2.6488305446114002, v_norm, 1e-9);
}

TEST(GSLVecDotTest, Normal0) {
  double v0_data[22] = {
      0.5018198337865563,  0.0150316926006699,  0.7958920963163227,
      -0.2257798350860565, -0.8869981063675993, -0.5980420396159223,
      -0.9495531184486903, 0.6351923438297407,  -0.9411472903777525,
      0.6236470284008762,  0.8562683937380398,  0.1579608810136111,
      0.7722065706465300,  0.4699189643263606,  0.3049555977992402,
      -0.9625500062303003, -0.0235151381472101, 0.5908819871326363,
      -0.5598601872344542, -0.4595995587998774, -0.3471574163172750,
      -0.8027721518482092};
  gsl_block v0_block = {22, v0_data};
  const gsl_vector v0 = {22, 1, v0_block.data, &v0_block, 0};
  double v1_data[22] = {
      0.2372844465456991,  0.4022338575055855,  -0.2579168977394961,
      -0.8016559359493407, -0.3223399735398202, -0.7700932365136059,
      -0.1689954321437372, -0.1260605663543299, -0.6463724884849176,
      -0.6704194113893538, -0.7212286905788854, -0.5683133053280716,
      0.3387311268734146,  0.0035149135983790,  -0.3437711840928095,
      -0.7843361553307167, -0.7759439792455367, -0.3401469101761194,
      -0.2047298042126491, 0.7044588101579374,  0.6850310575794785,
      -0.2719127601990134};
  gsl_block v1_block = {22, v1_data};
  const gsl_vector v1 = {22, 1, v1_block.data, &v1_block, 0};
  double v_dot = gsl_vector_dot(&v0, &v1);

  EXPECT_NEAR(0.9125257564991949, v_dot, 1e-9);
}

TEST(GSLMatTraceTest, Normal1) {
  double m_data[9] = {
      0.4778140485953484,  -0.0598314931599750, 0.6884527163171945,
      -0.7217917102564537, -0.9971225871556719, 0.9613479693682221,
      -0.5938857891136853, -0.3106930285082081, -0.2016570881627786};
  gsl_block m_block = {9, m_data};
  const gsl_matrix m = {3, 3, 3, m_block.data, &m_block, 0};
  double m_trace = gsl_matrix_trace(&m);

  EXPECT_NEAR(-0.7209656267231022, m_trace, 1e-9);
}

TEST(GSLMatDiagTest, Normal1) {
  double m_data[484] = {
      0.7392944167052278,  -0.2785042299408627, -0.0229652425406806,
      0.6255106490705395,  0.1786154499414507,  0.7793685525938296,
      0.1368778747829364,  -0.0711698204366213, -0.0086317451795681,
      -0.7904799596754584, 0.3474771705383828,  -0.4489659363916512,
      -0.6504197975908903, -0.3610934919499715, 0.5607536433886664,
      0.5688424014751889,  -0.6665030464928268, 0.1447656309214067,
      0.2815705100933237,  0.1965072521055944,  -0.3385377428913545,
      0.7170382591937352,  0.9369989621027746,  0.6548898738173541,
      0.0800406909422593,  0.9774699450579134,  0.4277092464882020,
      -0.5735947531081560, -0.1909752975003844, 0.5246089960737454,
      0.0147115955659718,  -0.9056828098986094, 0.0774432147317772,
      0.6880908455697006,  -0.3536801560033955, -0.1682882113678223,
      -0.7298811425557814, 0.9282670835958706,  0.7801410833280593,
      -0.7832936459601016, 0.4704754533938411,  0.7691550488602148,
      -0.6995802064818673, 0.8148290355957415,  -0.6186677978433355,
      0.7947375851361873,  0.6050925090746881,  0.8610684119754286,
      -0.3632365982959869, 0.8978032779394896,  -0.5295122853794691,
      0.5177067180307189,  0.0068483151660319,  0.1434248436238714,
      -0.0118402634776640, -0.2319944682792190, 0.0867216431927567,
      -0.1201663295150497, 0.9779069131818043,  -0.2855714742966673,
      0.7993279599342769,  0.8267323506025690,  -0.4820479424670043,
      0.8186290698989662,  0.2556380245472623,  0.5927498719656625,
      0.3540488351351316,  0.4864384373113260,  -0.5317661135611589,
      0.8694007192219890,  0.5662873013060699,  -0.8738325431669869,
      0.4484936053341859,  0.7341609439593113,  0.7816236714696858,
      0.8301356775407849,  0.6963120065640798,  0.2824842285563374,
      -0.4160408370483861, -0.6114221478835926, 0.1523435468206760,
      0.4428799268866348,  -0.1229447015805785, -0.4630896978405428,
      0.1748472228561222,  0.0945126221080150,  -0.8860289541383828,
      -0.4798280837120341, -0.5837095319975620, -0.8380039082895288,
      0.2089759812159522,  0.0762356582174983,  -0.4741518531657776,
      0.8912994405452330,  0.6997935381694302,  0.6829008578768252,
      0.7936302358387870,  -0.3344661840275676, -0.7263963064473413,
      0.3844308467821393,  -0.8327246323405437, 0.6524097172500911,
      -0.9340047718573692, 0.9784210854088435,  -0.3245529766435649,
      -0.6347025929098731, 0.9132668800889023,  0.1544112473752504,
      -0.1730429508923703, 0.2708420393442323,  -0.7763251672105700,
      0.3097424402720808,  -0.3560794564885215, -0.5988481342705065,
      0.6898690415196225,  0.9844155588767791,  -0.3566405592988697,
      0.9983779183614392,  0.1467842407065463,  0.0474288901481563,
      0.3706255245206242,  -0.1539937205466881, 0.1153350233166208,
      -0.2475283371741537, -0.2640376975941507, -0.6239045704111352,
      -0.0859699957804489, 0.2728499055224709,  0.7910469636214725,
      -0.4813176059499529, -0.4691783433651762, 0.5533874544593378,
      -0.6582453684679499, -0.6884035257626460, -0.9973329677761826,
      0.8963891334930463,  -0.0425132650383964, 0.9108494076867282,
      -0.7848833400331017, 0.3945894902672786,  -0.9113242183637216,
      0.3337308164725683,  -0.8583736330570084, 0.7996493359438313,
      0.5051534013653307,  0.6515616834724258,  0.6254299461158728,
      -0.0624111365993389, 0.3156328447725847,  0.6301585116722517,
      0.8666725896344352,  -0.5638090352309415, -0.8054134231458367,
      0.7853273115916355,  -0.3628633158773216, -0.7236411963624896,
      -0.0263340917122770, -0.3799845733877527, -0.2077779692885229,
      -0.4654028215431572, 0.8528331192070229,  0.4852045432166532,
      -0.6712419366055846, 0.9123598413300342,  -0.5159344990238994,
      0.9481554166731301,  -0.4499800957852127, 0.4231365781268710,
      -0.4642512577444522, 0.6788589197943256,  0.5702316664166165,
      -0.3476322941084966, -0.2020947093194583, -0.0897011290521437,
      0.5119173943463122,  0.8933459186292685,  0.9089834444678941,
      -0.1036554990302125, 0.0136878713610302,  -0.9281861104671731,
      -0.7659980925071974, -0.3308609400685438, 0.6742369871227898,
      -0.3670048513029052, 0.2335585645062992,  0.4473502588900613,
      -0.4011593976840282, -0.0051087153966600, -0.8268038914987141,
      -0.3179433616602723, 0.4141764825514924,  -0.8517109755427212,
      0.4078156500853527,  0.2787685578406027,  0.5845432352457445,
      0.1338970483118325,  -0.5031435168630518, 0.4807327628062048,
      -0.9441426456939486, 0.3370570411070881,  0.4173545086409260,
      0.7460652435654154,  -0.7048079908141596, -0.9879708752173813,
      -0.9849472133967649, -0.1735341363914757, 0.3290432232434917,
      0.8861917631052856,  0.2840546434301676,  -0.4523044227688546,
      -0.1908696126263900, -0.5414440610723601, 0.4770013628034642,
      -0.1781980856741117, 0.8810783584989126,  -0.5800679123090107,
      -0.7166455764772535, 0.7812620457902812,  -0.9328065535037322,
      0.0594416425787185,  -0.3519215047913173, -0.6832615953191601,
      0.9039926034139012,  -0.9997652786417290, -0.6984183735958076,
      0.8583057975427155,  0.2149840027498287,  -0.3422556928968137,
      -0.8578321987045006, -0.8727397929083669, 0.3393439238765741,
      0.0930298295718273,  -0.6656492021659237, -0.9047789621445452,
      0.8116790000234004,  0.7127813455973886,  0.8749989430331648,
      0.9386916079676948,  0.5247514617010127,  0.6665749566616146,
      0.1089441084430545,  0.5369086755670800,  0.4721185627728113,
      0.3800037929919280,  0.9817550822339103,  0.8570964924716704,
      -0.7337342763451460, -0.1938485170365383, -0.1502379639188274,
      0.1311767146103733,  -0.1290858755831279, 0.7072108435140081,
      -0.2334820920159404, 0.4251645407942539,  0.9784212702648944,
      -0.7050586585456948, 0.6881845817199630,  0.4776718904266422,
      0.6797791469296055,  -0.3787731303830468, 0.8197391588537519,
      -0.4731793730537315, -0.4973924985486893, 0.1684203886610240,
      -0.1838419652618914, -0.8865404743760299, -0.3103284424823467,
      0.3499203249183473,  -0.2727409267797585, 0.6692728927693632,
      0.8544028489064766,  0.7430265725366509,  -0.9203880989798663,
      -0.9043258497340436, -0.8877232465961318, 0.3193579347660636,
      -0.8352041606105456, -0.9404032975534320, -0.2767369977059986,
      -0.3491450805711762, 0.6780880565931580,  -0.3216692627785367,
      -0.4898961445874253, 0.7556201971731584,  -0.4944912534322534,
      -0.6839533981660226, 0.1934534598003528,  -0.2369853726141355,
      -0.2546758907651276, 0.0066265054475780,  -0.5821172137311839,
      -0.9376456790571417, -0.6894011132876126, 0.3561297939584787,
      -0.7737197344024416, -0.7645814232667507, 0.2613587392493000,
      0.1884729221074122,  -0.9590144359189827, -0.1781360314194576,
      0.0343775528365018,  -0.9776383180119288, 0.5177502544499486,
      0.7818112712584868,  0.1048558450921258,  -0.8363640031996620,
      -0.3848589707042960, -0.1601894068841210, 0.6581974625332765,
      0.4679751801889254,  -0.3450568364528703, 0.8755492097201634,
      0.5003114109407243,  -0.5226982280948396, 0.0089960751980342,
      0.3420277905085189,  0.9998089539192976,  0.7782009635556946,
      -0.9165995169833414, -0.5654449704541045, 0.5106874313086625,
      0.9253218633454419,  0.1629312959728475,  0.8123547838501166,
      0.0921845356380242,  -0.3667268052856405, 0.9242424981485318,
      -0.7639083724363585, -0.3103686769944483, 0.5325360766948402,
      0.6714563719665763,  -0.8216758108595013, 0.6196509543058106,
      -0.1990267019469585, 0.5415422939924490,  -0.9787472846365177,
      0.2432484587866901,  -0.9867874698814161, -0.6370367069788818,
      -0.6594139879781191, 0.4830401037156167,  -0.9846944251200980,
      0.7221334737822780,  -0.2016723997758005, -0.0885853546764193,
      0.6385452707050261,  0.9232168059097821,  0.2177322247418358,
      0.9122715963502814,  -0.2324317564427187, -0.0862115176123155,
      0.3287823130086405,  0.6729171318186675,  0.9728789724275477,
      -0.1426559762558552, 0.8550309242257355,  -0.3608205815716192,
      -0.9135249149624691, 0.4257682991414249,  -0.8935125901334866,
      0.7580794178326602,  0.3784640351552149,  -0.5415880406464073,
      -0.6704457107070751, 0.3754104767905442,  -0.8158399461636343,
      -0.6059724753538160, 0.7589517598109956,  -0.3154948622537805,
      0.0893780782819287,  0.5799280200999213,  -0.5888240094156796,
      -0.5311689300610885, -0.6325717442288228, -0.5971883395533659,
      -0.1017224809248931, -0.9669037767104007, 0.2567825063477913,
      0.2568876140612077,  0.7257252752841252,  0.7952265343336373,
      0.7544742875047181,  0.4264013913211424,  -0.0435540919377739,
      -0.5674406404851284, -0.4246559547685449, 0.9980211620158148,
      -0.3240597534544443, -0.6697314303043371, -0.4776683990167558,
      -0.2805449521912180, -0.8809363795927085, -0.6605622487142981,
      -0.7630421892062711, 0.2477228400490183,  0.9489300066597546,
      -0.4397405050026206, -0.6159231126641795, -0.3838818620123632,
      0.7095981804947977,  -0.1897919399906867, -0.3549947713199082,
      -0.6430889771733812, -0.0973162424213920, -0.7804084699678975,
      -0.9933388414904851, -0.4637524658552958, 0.6520357142002147,
      0.8814724577150059,  -0.5256177985890369, 0.5294508026386400,
      -0.3137448994398406, -0.5482804377276447, 0.6706151412986765,
      0.3042959810847168,  0.1582617339432160,  0.4098332525215012,
      0.2681581743469743,  -0.6321825916919963, -0.1632597244988048,
      0.6017106180217549,  -0.8557549479982967, -0.2323179107372433,
      0.7398554403311099,  -0.5613097167199070, 0.2202987481367391,
      -0.1311768273397793, 0.9549240218581962,  0.1246396933376990,
      -0.8820289406904060, 0.9824687222710027,  0.7434415662858751,
      0.7505048561176924,  0.5272088603376204,  0.5110861330953875,
      -0.5311061206969749, 0.9423250946662296,  0.9148739261040650,
      0.4852767024751019,  0.4184517339645673,  -0.8057096763738516,
      0.1481661779096808,  -0.5983296597751775, 0.4206967915512552,
      -0.5576028221657212, -0.1740218165119618, 0.8328195152302480,
      0.1124475685625832,  0.0733584347132794,  -0.9733076889711267,
      0.2390732060256120,  0.8558807004184910,  0.9036864074035296,
      -0.5604447555886014, -0.0273353126958942, -0.2317801320548354,
      -0.2137693106080421, -0.1360765394398662, -0.2563193552296719,
      -0.4082508668716549, 0.2361765103964635,  0.5329630372138685,
      -0.7253750349373895, -0.6135830796424688, 0.2788989695395412,
      -0.6539353521661624, 0.5052940080867308,  -0.4648734790850455,
      0.2018834673729264,  -0.6548314123345518, 0.5332480347844235,
      -0.6120405291546298, 0.2239945517999371,  0.0473870050963132,
      -0.3621604509148428, 0.6233925599186674,  -0.6743653225123754,
      -0.3216560669296342, -0.9985179005315223, 0.2748859906535102,
      -0.1010222757463710, 0.5489053577662562,  -0.2220085757628856,
      -0.3997239481874193};
  gsl_block m_block = {484, m_data};
  const gsl_matrix m = {22, 22, 22, m_block.data, &m_block, 0};
  double v_out_data[22] = {
      0.7392944167052278,  0.6548898738173541,  0.6050925090746881,
      0.8694007192219890,  -0.4741518531657776, 0.9844155588767791,
      -0.7848833400331017, 0.4852045432166532,  0.2335585645062992,
      0.8861917631052856,  0.3393439238765741,  0.4251645407942539,
      -0.8352041606105456, -0.1781360314194576, 0.1629312959728475,
      0.6385452707050261,  -0.3154948622537805, -0.8809363795927085,
      0.3042959810847168,  0.9148739261040650,  0.2361765103964635,
      -0.3997239481874193};
  gsl_block v_out_block = {22, v_out_data};
  gsl_vector v_out = {22, 1, v_out_block.data, &v_out_block, 0};
  gsl_matrix_diag(&m, &v_out);

  EXPECT_NEAR(0.7392944167052278, v_out.data[0], 1e-9);
  EXPECT_NEAR(0.6548898738173541, v_out.data[1], 1e-9);
  EXPECT_NEAR(0.6050925090746881, v_out.data[2], 1e-9);
  EXPECT_NEAR(0.8694007192219890, v_out.data[3], 1e-9);
  EXPECT_NEAR(-0.4741518531657776, v_out.data[4], 1e-9);
  EXPECT_NEAR(0.9844155588767791, v_out.data[5], 1e-9);
  EXPECT_NEAR(-0.7848833400331017, v_out.data[6], 1e-9);
  EXPECT_NEAR(0.4852045432166532, v_out.data[7], 1e-9);
  EXPECT_NEAR(0.2335585645062992, v_out.data[8], 1e-9);
  EXPECT_NEAR(0.8861917631052856, v_out.data[9], 1e-9);
  EXPECT_NEAR(0.3393439238765741, v_out.data[10], 1e-9);
  EXPECT_NEAR(0.4251645407942539, v_out.data[11], 1e-9);
  EXPECT_NEAR(-0.8352041606105456, v_out.data[12], 1e-9);
  EXPECT_NEAR(-0.1781360314194576, v_out.data[13], 1e-9);
  EXPECT_NEAR(0.1629312959728475, v_out.data[14], 1e-9);
  EXPECT_NEAR(0.6385452707050261, v_out.data[15], 1e-9);
  EXPECT_NEAR(-0.3154948622537805, v_out.data[16], 1e-9);
  EXPECT_NEAR(-0.8809363795927085, v_out.data[17], 1e-9);
  EXPECT_NEAR(0.3042959810847168, v_out.data[18], 1e-9);
  EXPECT_NEAR(0.9148739261040650, v_out.data[19], 1e-9);
  EXPECT_NEAR(0.2361765103964635, v_out.data[20], 1e-9);
  EXPECT_NEAR(-0.3997239481874193, v_out.data[21], 1e-9);
}

TEST(GSLVecSaturateTest, Bounded1) {
  double v0_data[22] = {
      0.6126492519761340,  0.7344660770002478,  -0.0570706674728567,
      0.2103397631555637,  0.4087857410623481,  0.2958549734350442,
      0.7855741307482806,  0.9026295071103760,  0.2853792511025530,
      -0.2905599344933709, 0.4341414751935415,  -0.9285019159963126,
      -0.0635515015243604, -0.7906789906537726, 0.6206193907597877,
      0.3927386094350362,  -0.0588678312470670, 0.0737020123861039,
      0.5601730577784074,  -0.3893951749709093, 0.1755916253803200,
      0.2124998804200655};
  gsl_block v0_block = {22, v0_data};
  const gsl_vector v0 = {22, 1, v0_block.data, &v0_block, 0};
  double v_out_data[22] = {
      0.6126492519761340, 0.7010979668475895, 0.0209638827083779,
      0.2103397631555637, 0.4087857410623481, 0.2958549734350442,
      0.7010979668475895, 0.7010979668475895, 0.2853792511025530,
      0.0209638827083779, 0.4341414751935415, 0.0209638827083779,
      0.0209638827083779, 0.0209638827083779, 0.6206193907597877,
      0.3927386094350362, 0.0209638827083779, 0.0737020123861039,
      0.5601730577784074, 0.0209638827083779, 0.1755916253803200,
      0.2124998804200655};
  gsl_block v_out_block = {22, v_out_data};
  gsl_vector v_out = {22, 1, v_out_block.data, &v_out_block, 0};
  gsl_vector_saturate(&v0, 0.0209638827083779, 0.7010979668475895, &v_out);

  EXPECT_NEAR(0.6126492519761340, v_out.data[0], 1e-9);
  EXPECT_NEAR(0.7010979668475895, v_out.data[1], 1e-9);
  EXPECT_NEAR(0.0209638827083779, v_out.data[2], 1e-9);
  EXPECT_NEAR(0.2103397631555637, v_out.data[3], 1e-9);
  EXPECT_NEAR(0.4087857410623481, v_out.data[4], 1e-9);
  EXPECT_NEAR(0.2958549734350442, v_out.data[5], 1e-9);
  EXPECT_NEAR(0.7010979668475895, v_out.data[6], 1e-9);
  EXPECT_NEAR(0.7010979668475895, v_out.data[7], 1e-9);
  EXPECT_NEAR(0.2853792511025530, v_out.data[8], 1e-9);
  EXPECT_NEAR(0.0209638827083779, v_out.data[9], 1e-9);
  EXPECT_NEAR(0.4341414751935415, v_out.data[10], 1e-9);
  EXPECT_NEAR(0.0209638827083779, v_out.data[11], 1e-9);
  EXPECT_NEAR(0.0209638827083779, v_out.data[12], 1e-9);
  EXPECT_NEAR(0.0209638827083779, v_out.data[13], 1e-9);
  EXPECT_NEAR(0.6206193907597877, v_out.data[14], 1e-9);
  EXPECT_NEAR(0.3927386094350362, v_out.data[15], 1e-9);
  EXPECT_NEAR(0.0209638827083779, v_out.data[16], 1e-9);
  EXPECT_NEAR(0.0737020123861039, v_out.data[17], 1e-9);
  EXPECT_NEAR(0.5601730577784074, v_out.data[18], 1e-9);
  EXPECT_NEAR(0.0209638827083779, v_out.data[19], 1e-9);
  EXPECT_NEAR(0.1755916253803200, v_out.data[20], 1e-9);
  EXPECT_NEAR(0.2124998804200655, v_out.data[21], 1e-9);
}

TEST(GSLVecNormBoundTest, Normal1) {
  double v0_data[22] = {
      -0.0474426594209401, -0.3846057556397970, 0.3567196864760862,
      0.7163078723652749,  0.8230853372628459,  -0.5923995690874002,
      -0.5536787316366334, -0.4480268960178087, 0.8308318581478780,
      -0.9936111210227514, -0.3975730187852329, 0.1938630987194752,
      0.4860077905986644,  -0.7779406049185791, 0.9297943456250559,
      0.5304219870191134,  0.4701851832453003,  -0.3785057591033900,
      -0.9518640096829316, -0.1206256025591379, -0.0526301457362273,
      0.6222725375454516};
  gsl_block v0_block = {22, v0_data};
  const gsl_vector v0 = {22, 1, v0_block.data, &v0_block, 0};
  double v_norm = gsl_vector_norm_bound(&v0, 0.6126499813021151);

  EXPECT_NEAR(2.8038535362982335, v_norm, 1e-9);
}

TEST(GSLVecNormBoundTest, Bounded1) {
  double v0_data[22] = {
      0.8994702470661189,  -0.0197808837977544, 0.9928313254924177,
      -0.2234807869994511, -0.9833946677443199, 0.7882508747285919,
      -0.3472775035867355, -0.4753712595155146, -0.2012012582669938,
      0.2080016761798777,  0.7220227179618304,  -0.4846781976055876,
      -0.3756404934511617, 0.7354474270284892,  -0.0031025467443351,
      0.9405147454254243,  -0.1128448015603778, -0.8762790718792546,
      -0.6588609711944513, -0.8987533030323547, 0.6944782592260068,
      -0.3408758376317624};
  gsl_block v0_block = {22, v0_data};
  const gsl_vector v0 = {22, 1, v0_block.data, &v0_block, 0};
  double v_norm = gsl_vector_norm_bound(&v0, 2.2652402385195196);

  EXPECT_NEAR(2.9677555607806578, v_norm, 1e-9);
}

TEST(GSLVecDotTest, Normal1) {
  double v0_data[22] = {
      -0.5203636045786271, -0.0511095676061033, 0.2735860791461300,
      -0.9893558546928862, 0.8017148374340222,  -0.9573009274954889,
      0.8819387312812332,  0.2412429036341626,  0.0596263066476228,
      0.5153249805473008,  -0.9674672080698599, -0.5024278658049348,
      -0.9417231719684274, 0.0068095973898388,  -0.0220216229693329,
      0.8491198548434991,  0.0474757318679011,  0.2227057069156813,
      0.9616523544468918,  0.6743598859207756,  0.6106897493172516,
      0.0313518159876975};
  gsl_block v0_block = {22, v0_data};
  const gsl_vector v0 = {22, 1, v0_block.data, &v0_block, 0};
  double v1_data[22] = {
      -0.7955040736047319, -0.6450375566643338, 0.0436715769077187,
      -0.1236733955260156, 0.0519088274456010,  -0.1786632143879343,
      0.3837006928965041,  -0.1264164281603029, 0.7658124971879248,
      -0.9216378656495250, -0.3389389342668858, -0.1243822837213466,
      0.4640678168394807,  0.0868106545649889,  -0.8631368204252552,
      0.3482194774130820,  0.1495117092282041,  0.4969245477027222,
      0.6829030833612928,  0.3804076525671629,  -0.8442791754143559,
      -0.0838762192175659};
  gsl_block v1_block = {22, v1_data};
  const gsl_vector v1 = {22, 1, v1_block.data, &v1_block, 0};
  double v_dot = gsl_vector_dot(&v0, &v1);

  EXPECT_NEAR(1.4539491454778972, v_dot, 1e-9);
}

TEST(GSLMatTraceTest, Normal2) {
  double m_data[9] = {
      0.2558163266318998,  -0.7464775155282530, -0.1147654817849784,
      -0.6874708217441652, -0.6689084484834174, -0.5910620493040035,
      -0.6264062377123747, -0.5094491514814981, -0.2368817634208205};
  gsl_block m_block = {9, m_data};
  const gsl_matrix m = {3, 3, 3, m_block.data, &m_block, 0};
  double m_trace = gsl_matrix_trace(&m);

  EXPECT_NEAR(-0.6499738852723380, m_trace, 1e-9);
}

TEST(GSLMatDiagTest, Normal2) {
  double m_data[484] = {
      -0.4240842330086925, 0.9346008440950588,  -0.3452269687624598,
      0.5396136661894180,  0.4473777639296135,  -0.9904667319859080,
      0.4833175263319502,  -0.5590415756768508, -0.0782213841861041,
      -0.5879660939084708, 0.7023997505476991,  0.0437031662216629,
      0.7593995167965277,  -0.8548859642364537, 0.1807143485814426,
      0.7622255225662449,  -0.6717531342091208, -0.3242832863305640,
      -0.1629662951517277, 0.1831227117959255,  0.7597008551215951,
      0.4636009537032328,  -0.7125174329043082, 0.8338336983546495,
      0.8486607699575324,  0.7758878782417360,  -0.9426074690668356,
      0.5478426275011283,  -0.8436264111578575, 0.1224460405092636,
      0.0616578930721146,  0.6020241750251341,  -0.9946903808255914,
      -0.9463009675522265, -0.9956466234278525, -0.0744746064139561,
      0.9831779885001866,  0.0281595156522720,  -0.0011241064626935,
      0.2263319895840346,  -0.3382348375084001, -0.9567514704840496,
      0.4169501085269756,  -0.7359051551890012, -0.2845123936850116,
      0.7947369651532248,  -0.5126880978556685, 0.1024160145870439,
      -0.1587892246147296, 0.3100407426879521,  -0.8394212572244599,
      0.5548762104954907,  0.0299892147323806,  -0.9400481152064184,
      -0.2439007407481864, -0.8765677868163460, -0.9162006409318304,
      0.0417691802188671,  -0.5293606931514121, 0.9990653638489897,
      0.2294531536634443,  0.4593659849495280,  -0.0924013614738881,
      0.8148340446845079,  -0.4623305172231016, -0.4725488347661753,
      -0.4437909514617164, 0.6739668795561682,  0.2315459736739498,
      0.4435958877967681,  0.3731546847437417,  0.4144121154294798,
      -0.8653317884428577, -0.8492071480060581, -0.4346281159196370,
      0.9683818264601043,  -0.6250084101335307, 0.0131108885415783,
      -0.0325713855099847, 0.0052946333988333,  0.5355639112504176,
      -0.5046935873378480, -0.9999494280668277, 0.9952112260514590,
      -0.6099051271060423, 0.4901853971701489,  -0.3340354929246385,
      0.7652202270480633,  0.4955711844144204,  -0.9557061021679232,
      0.2116590125123299,  -0.1086564466095832, 0.7672049158233878,
      0.9066141392042073,  0.3712650756389468,  -0.5121165833829309,
      0.8190211443033952,  -0.8941390028992506, -0.0382060350276217,
      -0.7438913434305576, -0.3451725884774099, -0.7930446418895178,
      0.6849658495554161,  -0.3570600089555287, -0.4904730071229133,
      0.7981006497603169,  -0.3108798410239495, 0.9455726327063929,
      -0.7752533819930281, 0.8139020853272509,  -0.0290966942374176,
      0.5567158297865846,  -0.9516145764515187, -0.5680261946973375,
      -0.9048022552959551, -0.5740984889831104, -0.8912965198004137,
      0.1998960546394237,  0.4814867783107870,  0.6026974605041078,
      0.9492573867191427,  0.3434885331813626,  -0.6745536096950111,
      -0.9922740282210696, -0.0685294097150932, -0.9197891732707526,
      -0.0331834154392388, 0.8752555730268328,  -0.8214420997638776,
      -0.0351539707267474, 0.7526458913218579,  -0.9597805163556610,
      -0.2773086785452918, -0.2760628450223632, 0.6232547365565073,
      -0.2927613619559675, 0.3857300317902925,  0.1434633896087074,
      0.2974812096533086,  0.3921535189891667,  -0.3028290049453091,
      -0.0886311813159784, 0.6969820736843015,  0.4102278528744021,
      0.0326845696949909,  -0.7247845538551627, -0.1751244364420363,
      -0.2926810630752030, 0.1859935886892496,  0.1100039058395272,
      0.6316998749882399,  -0.5841051934893724, 0.2488576465391981,
      -0.9568916848792957, -0.8151689711272325, 0.2788149135269025,
      -0.9085819919594331, 0.2844768162322564,  -0.7580114876690300,
      -0.7115293378964074, 0.6850548627013511,  0.2532299864151240,
      0.1768124454361681,  -0.8558224344378991, 0.4221164241528790,
      0.8805471439706187,  -0.5994888872635027, 0.1395517240820978,
      -0.2245181368994498, -0.9620382927787867, -0.3661117331466943,
      0.7496519229017027,  0.9958792209586014,  0.8145462281812372,
      -0.9315637224498940, -0.7956006484914895, 0.4376367808906467,
      -0.9636522275013737, -0.8741247456500094, 0.8293062472342918,
      0.2141343844987540,  -0.2961259781491727, 0.1303889542008494,
      0.2259712133915517,  -0.2484984856903549, -0.4045072202786570,
      -0.3596960081234288, -0.5521916442019388, -0.3502395159589060,
      0.8402973453306137,  0.4695557000508626,  0.9205105525834723,
      0.6843563796143031,  0.3401544262530563,  -0.7079771841918534,
      0.0868087208553301,  -0.8088015960013792, -0.4573364323044247,
      -0.0620502817066042, 0.1331715233425470,  0.7694339661057426,
      0.5987266649076270,  0.1296420431612024,  -0.2738953056012818,
      -0.8141280699783751, -0.9492344795279559, -0.5935729743212439,
      0.6302616468340054,  -0.7288262288568024, -0.2843156435845902,
      -0.6569427657571214, -0.9645953801617726, -0.0868949598138307,
      0.2463128629761429,  -0.9576663460261579, -0.0781431961654839,
      0.5281533276971453,  -0.0388861101660567, 0.3447436844648082,
      -0.1485319743116018, -0.8421373389027509, 0.8019791785578667,
      0.3853457966099691,  0.6406410222319316,  0.9189955477080620,
      0.0361561519514955,  0.0302503896199875,  -0.5052367750858280,
      -0.4148402490194507, -0.5527839849755161, -0.5796149327379121,
      -0.9847514846798333, -0.1418968624644088, 0.4949342401878045,
      0.2741851546438758,  -0.2704520951296421, -0.9540471968206057,
      -0.4242227911134611, -0.4624054134059912, -0.6217923846969469,
      0.2338566468958097,  0.0863864881644352,  0.6663168379221767,
      0.6123971758424498,  -0.7771885557087086, 0.7677132600124041,
      -0.3442183135020029, 0.5527863153090096,  0.5935564049126745,
      -0.5475774693342412, 0.6282600144015733,  -0.1058442899014300,
      -0.1455795501769679, -0.1756165518372965, 0.6247576085731952,
      -0.1998357191849807, -0.9395537125857720, 0.7260433533652026,
      -0.5763213706881216, 0.7076823947678106,  0.0868908852259824,
      0.1386893270122238,  0.2558978242164798,  0.6263844125718583,
      -0.5231012841127483, 0.5292929872464860,  -0.4388264017282093,
      0.0990761236201367,  -0.4091278056322416, -0.8077991238041644,
      0.5079560353537382,  0.2176821375403086,  -0.5482440011742178,
      0.7880978384787809,  0.4971147117023527,  0.0293362035399500,
      0.7847415230573362,  -0.4699342534129838, -0.6254790565049011,
      0.5195421882627813,  -0.1591210061156758, -0.6948103564086869,
      0.9098479194713180,  -0.7049248627284064, 0.1085405969559430,
      0.6619454104884699,  0.5881840174385256,  -0.4785373304051828,
      0.4558419261995983,  -0.7815463106892038, -0.1469393747384098,
      0.8949736975228539,  0.7406378516092644,  -0.8575108700071270,
      0.3901449348065045,  -0.0726788147470605, -0.3552901440847234,
      -0.0457463783745555, -0.0912514967997951, 0.1206977849541890,
      -0.6925528939466816, -0.1838588164901567, -0.0055450587254131,
      0.2483934137622374,  -0.4648892170687389, 0.4983691888555308,
      0.3655552122675934,  -0.6402129418085232, 0.6482233665521313,
      -0.4381089607876607, 0.2477905520872026,  -0.3448686046238647,
      0.0249018262437204,  0.2843553481569867,  -0.4411340658511975,
      0.2828760858991139,  -0.1786281068599485, -0.2725636913010485,
      0.8210612658046434,  0.4119740221135340,  0.4264724364750425,
      0.7472340796806078,  0.3598048816818096,  -0.1075940672017186,
      0.7763566746689190,  0.5735710208747760,  0.3451031115494498,
      -0.4895534635349235, 0.9429903544423757,  -0.6764992137955061,
      -0.7256822160381176, -0.9297481397566094, -0.2123972909334799,
      0.5100860392338855,  -0.7986624775392874, 0.6226250694414008,
      -0.9967394354944030, 0.4521948604735389,  0.7350408409402285,
      0.0440953231773156,  0.8580331720039456,  0.3280127653486116,
      0.8409634250436200,  0.9513878870945605,  0.8834105783461450,
      -0.0282362842439785, -0.2279042473158102, 0.3082797590256416,
      -0.0767157983174385, -0.6057379149422344, 0.8074327310680161,
      -0.9146635028029626, -0.9201754465249401, 0.4208323865850216,
      -0.2791662957771219, 0.2906901470415526,  -0.8340221163790416,
      0.1187724342124021,  0.8257119775399737,  -0.5096358710786753,
      0.1908152857978966,  -0.2243251092754037, -0.0904799865426431,
      0.2421450280486086,  0.2920849893201807,  -0.6650133458925320,
      -0.3419938976526125, 0.2920674378173924,  -0.9372649162738147,
      -0.8724184473032639, 0.0793570646195600,  0.9318355459545726,
      -0.7614468491449906, 0.4607022747071750,  0.9397490489427140,
      0.9551906861414747,  0.9415051716892573,  0.3752257284434113,
      0.3530492848092737,  0.3761507171224328,  0.4401783186744939,
      0.6715741981746000,  -0.6815783701826705, -0.4227932154939156,
      0.3246026678927694,  -0.8173297668101331, -0.8499887665574646,
      -0.7138954511649391, -0.0939619143279544, 0.9058862428745307,
      0.4958803310627002,  0.6187441131633116,  0.6950075082193270,
      0.6726458355631060,  0.3157898100034524,  0.2214359324988560,
      0.0742866756792857,  0.5570209760810469,  -0.7857595290536865,
      0.4620578245214126,  -0.5826148515088365, -0.3205561533806554,
      0.6437408508611040,  0.9001475325995474,  0.6211585663669401,
      0.8979025632574826,  -0.1174410057544337, -0.3000657279204451,
      -0.5202824297132036, -0.6193888143977264, 0.2362686642867331,
      0.2552559461597399,  -0.1473984521999328, -0.7480013337510887,
      0.6020616591765151,  0.7950950022269663,  0.5664365077288831,
      -0.1805957517930052, -0.3410321180808717, 0.8584172537842634,
      0.3017467544923169,  0.0323252276578438,  -0.0192845447595373,
      0.3969979186262369,  0.2345465030509171,  -0.2595974474986644,
      -0.5065024754714071, 0.8165461704772250,  -0.4963406088219333,
      0.6160988521300801,  -0.2807972122668778, -0.9240789349889980,
      0.7220377453598541,  -0.0444558939862263, -0.2525922915603391,
      0.1741818219642619,  -0.5332568280124927, -0.1129864481967178,
      0.7249839633033914,  0.4800794112013538,  0.1944634914418306,
      -0.8133131253457468, -0.9516657807997062, 0.5776565102947502,
      0.1415072779864170,  -0.2587894357973379, -0.9211583800000434,
      0.2181930639116936,  0.0186501675608723,  -0.5563299208222701,
      0.6352168929518587,  -0.8841593808159371, 0.8958137842465088,
      0.4744553131060507,  0.6693452098786703,  0.7298459177794947,
      0.5600648028227504,  -0.1398465422345698, -0.6013070364474087,
      -0.9386581783199173, 0.1659179954227661,  0.4401456638848431,
      0.5539577969703007,  -0.8596785887550957, -0.0937574477480665,
      0.2921910001580910,  0.2229603655343637,  0.5797117490167663,
      0.2234640684846656,  0.4552817592226774,  -0.8085448428771753,
      -0.6009012329192769, 0.4862322449983985,  -0.5131915004046017,
      0.5257801379118276,  -0.1040192768909076, -0.9853276874000878,
      0.1010724084258454,  0.1676674186778659,  -0.2460904011937619,
      0.4940064886147844,  0.2767876525725803,  0.1024700286409954,
      0.9495624845262263};
  gsl_block m_block = {484, m_data};
  const gsl_matrix m = {22, 22, 22, m_block.data, &m_block, 0};
  double v_out_data[22] = {
      -0.4240842330086925, 0.8338336983546495,  -0.5126880978556685,
      0.4435958877967681,  0.7672049158233878,  -0.5740984889831104,
      0.2974812096533086,  0.2532299864151240,  -0.2484984856903549,
      0.6302616468340054,  -0.5796149327379121, -0.1756165518372965,
      0.7847415230573362,  0.1206977849541890,  0.3598048816818096,
      -0.0282362842439785, -0.9372649162738147, 0.6187441131633116,
      0.6020616591765151,  -0.1129864481967178, 0.1659179954227661,
      0.9495624845262263};
  gsl_block v_out_block = {22, v_out_data};
  gsl_vector v_out = {22, 1, v_out_block.data, &v_out_block, 0};
  gsl_matrix_diag(&m, &v_out);

  EXPECT_NEAR(-0.4240842330086925, v_out.data[0], 1e-9);
  EXPECT_NEAR(0.8338336983546495, v_out.data[1], 1e-9);
  EXPECT_NEAR(-0.5126880978556685, v_out.data[2], 1e-9);
  EXPECT_NEAR(0.4435958877967681, v_out.data[3], 1e-9);
  EXPECT_NEAR(0.7672049158233878, v_out.data[4], 1e-9);
  EXPECT_NEAR(-0.5740984889831104, v_out.data[5], 1e-9);
  EXPECT_NEAR(0.2974812096533086, v_out.data[6], 1e-9);
  EXPECT_NEAR(0.2532299864151240, v_out.data[7], 1e-9);
  EXPECT_NEAR(-0.2484984856903549, v_out.data[8], 1e-9);
  EXPECT_NEAR(0.6302616468340054, v_out.data[9], 1e-9);
  EXPECT_NEAR(-0.5796149327379121, v_out.data[10], 1e-9);
  EXPECT_NEAR(-0.1756165518372965, v_out.data[11], 1e-9);
  EXPECT_NEAR(0.7847415230573362, v_out.data[12], 1e-9);
  EXPECT_NEAR(0.1206977849541890, v_out.data[13], 1e-9);
  EXPECT_NEAR(0.3598048816818096, v_out.data[14], 1e-9);
  EXPECT_NEAR(-0.0282362842439785, v_out.data[15], 1e-9);
  EXPECT_NEAR(-0.9372649162738147, v_out.data[16], 1e-9);
  EXPECT_NEAR(0.6187441131633116, v_out.data[17], 1e-9);
  EXPECT_NEAR(0.6020616591765151, v_out.data[18], 1e-9);
  EXPECT_NEAR(-0.1129864481967178, v_out.data[19], 1e-9);
  EXPECT_NEAR(0.1659179954227661, v_out.data[20], 1e-9);
  EXPECT_NEAR(0.9495624845262263, v_out.data[21], 1e-9);
}

TEST(GSLVecSaturateTest, Bounded2) {
  double v0_data[22] = {
      0.1087389732398125,  -0.1541478427359033, 0.7461893847948071,
      0.1396995920205566,  -0.2998742251944020, 0.4297648101354175,
      -0.6318610334979700, -0.3622577425861160, 0.2447367814967920,
      -0.9731436586889552, -0.2093948167953263, 0.1271726956424502,
      0.4968353732354358,  -0.0366268257366418, -0.4357469432052288,
      -0.9673476842384383, -0.8508888091565536, 0.4330239908245956,
      0.4961155586286909,  -0.1657027326021321, 0.6015445038533522,
      0.6378881040935342};
  gsl_block v0_block = {22, v0_data};
  const gsl_vector v0 = {22, 1, v0_block.data, &v0_block, 0};
  double v_out_data[22] = {
      -0.1703725935576007, -0.1703725935576007, -0.1703725935576007,
      -0.1703725935576007, -0.2998742251944020, -0.1703725935576007,
      -0.6318610334979700, -0.3622577425861160, -0.1703725935576007,
      -0.7600892290787110, -0.2093948167953263, -0.1703725935576007,
      -0.1703725935576007, -0.1703725935576007, -0.4357469432052288,
      -0.7600892290787110, -0.7600892290787110, -0.1703725935576007,
      -0.1703725935576007, -0.1703725935576007, -0.1703725935576007,
      -0.1703725935576007};
  gsl_block v_out_block = {22, v_out_data};
  gsl_vector v_out = {22, 1, v_out_block.data, &v_out_block, 0};
  gsl_vector_saturate(&v0, -0.7600892290787110, -0.1703725935576007, &v_out);

  EXPECT_NEAR(-0.1703725935576007, v_out.data[0], 1e-9);
  EXPECT_NEAR(-0.1703725935576007, v_out.data[1], 1e-9);
  EXPECT_NEAR(-0.1703725935576007, v_out.data[2], 1e-9);
  EXPECT_NEAR(-0.1703725935576007, v_out.data[3], 1e-9);
  EXPECT_NEAR(-0.2998742251944020, v_out.data[4], 1e-9);
  EXPECT_NEAR(-0.1703725935576007, v_out.data[5], 1e-9);
  EXPECT_NEAR(-0.6318610334979700, v_out.data[6], 1e-9);
  EXPECT_NEAR(-0.3622577425861160, v_out.data[7], 1e-9);
  EXPECT_NEAR(-0.1703725935576007, v_out.data[8], 1e-9);
  EXPECT_NEAR(-0.7600892290787110, v_out.data[9], 1e-9);
  EXPECT_NEAR(-0.2093948167953263, v_out.data[10], 1e-9);
  EXPECT_NEAR(-0.1703725935576007, v_out.data[11], 1e-9);
  EXPECT_NEAR(-0.1703725935576007, v_out.data[12], 1e-9);
  EXPECT_NEAR(-0.1703725935576007, v_out.data[13], 1e-9);
  EXPECT_NEAR(-0.4357469432052288, v_out.data[14], 1e-9);
  EXPECT_NEAR(-0.7600892290787110, v_out.data[15], 1e-9);
  EXPECT_NEAR(-0.7600892290787110, v_out.data[16], 1e-9);
  EXPECT_NEAR(-0.1703725935576007, v_out.data[17], 1e-9);
  EXPECT_NEAR(-0.1703725935576007, v_out.data[18], 1e-9);
  EXPECT_NEAR(-0.1703725935576007, v_out.data[19], 1e-9);
  EXPECT_NEAR(-0.1703725935576007, v_out.data[20], 1e-9);
  EXPECT_NEAR(-0.1703725935576007, v_out.data[21], 1e-9);
}

TEST(GSLVecNormBoundTest, Normal2) {
  double v0_data[22] = {
      -0.0487106137772402, -0.2765600680235407, 0.1251194451811175,
      -0.2682615874098442, 0.2198861886354115,  0.0126082779977112,
      0.6073752529349570,  -0.9739361116084018, 0.2353023483537191,
      -0.5918096530285701, -0.7082263119708145, 0.7663319751474258,
      0.2683279089806279,  -0.7474368562314486, -0.3014593445537446,
      -0.9092650785116621, 0.2476755744789521,  0.1796957725881521,
      -0.6951292484789806, 0.9856991797385120,  -0.8772309679492107,
      -0.5570978400671667};
  gsl_block v0_block = {22, v0_data};
  const gsl_vector v0 = {22, 1, v0_block.data, &v0_block, 0};
  double v_norm = gsl_vector_norm_bound(&v0, 0.9639158721685229);

  EXPECT_NEAR(2.6839399189020292, v_norm, 1e-9);
}

TEST(GSLVecNormBoundTest, Bounded2) {
  double v0_data[22] = {
      -0.9919274799443534, -0.1507311563643792, 0.2013628754520331,
      -0.0867098552193386, 0.6683325698160529,  -0.9231392978244002,
      0.4155540470121639,  -0.3316357067195097, 0.9795843994640328,
      0.7977277620278165,  0.5242614250279445,  -0.0422234101242835,
      -0.5295694187541280, 0.8479028735311325,  -0.1299785984179835,
      -0.8539885287614328, 0.7405644718586546,  -0.5877887322406290,
      -0.0218382341353895, 0.0956189301666133,  0.2080214746333844,
      0.7610142439522225};
  gsl_block v0_block = {22, v0_data};
  const gsl_vector v0 = {22, 1, v0_block.data, &v0_block, 0};
  double v_norm = gsl_vector_norm_bound(&v0, 2.5677685563977217);

  EXPECT_NEAR(2.7893290183570500, v_norm, 1e-9);
}

TEST(GSLVecDotTest, Normal2) {
  double v0_data[22] = {
      -0.1772971276373567, -0.6337365372209542, -0.0333336358547727,
      -0.1473944292386207, -0.2636638914894178, 0.3032838416357293,
      -0.8352689973211527, 0.0427219430914512,  0.5874713499274944,
      0.9500741503201207,  0.4894724629367782,  -0.7939857080426607,
      -0.4157433596625408, -0.2644817291747448, -0.0636948255652801,
      0.3213676293447809,  -0.0668036883750160, -0.9948622466714208,
      0.5974217016426617,  0.7215668423248280,  -0.7107915215734624,
      -0.7251729714222293};
  gsl_block v0_block = {22, v0_data};
  const gsl_vector v0 = {22, 1, v0_block.data, &v0_block, 0};
  double v1_data[22] = {
      0.7056031531256879,  0.7506570338880052,  -0.6824140765337994,
      0.1287708076937328,  0.4489000761793938,  0.9619801033612647,
      -0.9300199414897499, -0.6106443794165441, 0.4890249962484916,
      0.2955587500219259,  0.3728113058088776,  -0.1016943365829570,
      -0.0602502027231895, -0.3750133733436822, 0.8353101418479383,
      0.0004892248893016,  -0.7869269419314553, -0.5199485496748060,
      -0.5504961262907320, -0.1567188806158275, -0.4180515834520107,
      0.9753071921712479};
  gsl_block v1_block = {22, v1_data};
  const gsl_vector v1 = {22, 1, v1_block.data, &v1_block, 0};
  double v_dot = gsl_vector_dot(&v0, &v1);

  EXPECT_NEAR(0.9473373769238338, v_dot, 1e-9);
}

TEST(GSLMatTraceTest, Normal3) {
  double m_data[9] = {
      0.5160279159931480,  0.5253793451979449,  0.5927547135809661,
      -0.3414411588115336, -0.2466755818772881, 0.8938373109571449,
      -0.0699745353568049, 0.2271149570396833,  0.8192509594925887};
  gsl_block m_block = {9, m_data};
  const gsl_matrix m = {3, 3, 3, m_block.data, &m_block, 0};
  double m_trace = gsl_matrix_trace(&m);

  EXPECT_NEAR(1.0886032936084487, m_trace, 1e-9);
}

TEST(GSLMatDiagTest, Normal3) {
  double m_data[484] = {
      0.5872732132667224,  -0.1938426250973626, 0.0562476698153549,
      -0.7628627079935237, -0.4518834865364278, 0.3071574348789297,
      -0.4633450794416574, 0.6936474292887405,  0.4513892942953914,
      -0.1423395516785313, 0.5552469015091492,  -0.1286361689525530,
      -0.5253332818236263, 0.7311869750797499,  -0.1439216148976263,
      0.9630737583444313,  -0.6459419116567640, -0.7030386955439700,
      0.5770848664098447,  -0.1755574746787096, 0.1811772074109819,
      0.7703745422759458,  -0.0540626069008117, 0.5634235580538283,
      0.0936442960333492,  -0.4740374398303215, 0.7173480752497186,
      -0.0539285296855847, 0.9499292114216169,  -0.9091902102718896,
      -0.9267708191637727, -0.0677689619994959, 0.1581102775137635,
      -0.6062031380260289, -0.8422591829384922, -0.5461392178099438,
      0.9812061808349364,  0.6214820261840930,  -0.7541946114957578,
      -0.9976723790992044, 0.9427144607800546,  -0.7718677402814185,
      -0.5958886028248236, -0.4057156083459019, -0.2187409273521177,
      -0.2593068770135210, 0.2459202008229535,  0.8828486284729293,
      -0.8276963420246510, 0.3019763413172125,  0.0192895116585243,
      0.4297886676672100,  0.4741086086694430,  0.9935404935410164,
      0.3698514189221818,  -0.4413095126750786, 0.7725142794158832,
      0.4239036950970727,  0.3231129527076015,  -0.5786789950864724,
      0.9053258829950319,  0.9014051457458530,  0.0314828793618382,
      -0.5517152787545931, 0.9762774752665309,  0.5757909873688420,
      0.5842016661049072,  -0.9890127415945198, -0.1561987508053810,
      0.5434339503086938,  -0.6329042950863313, -0.9156841812778311,
      -0.2688783535019730, 0.6442571156702621,  0.4157260870213648,
      0.4657615737167453,  -0.5375774841223637, 0.1636868435087098,
      -0.8729315837135483, 0.2492720262380457,  -0.8354806390621279,
      -0.5301027187292691, -0.9375970708480754, 0.1273904691963066,
      -0.7554373305630839, 0.3669332167303916,  0.5416380238419656,
      -0.5746181751318700, 0.4984343833004776,  0.9644743942848497,
      0.5765146493831925,  0.8590118546216019,  -0.2809075789157727,
      0.7030876871365304,  -0.0853227213778911, -0.8687296460487448,
      -0.2942997440456723, -0.5255308810904142, 0.0350096256055827,
      0.8321361242207270,  -0.2085850205472846, 0.5859039690432601,
      -0.9072202386059556, 0.6399386186613785,  -0.5205341335613756,
      0.0293655060052180,  -0.4004676368384681, -0.4508601361577573,
      -0.6536786758728090, 0.3047936479597617,  0.3911055091451499,
      0.2826615604708849,  -0.6339139640288378, -0.2400013020579956,
      -0.4129676326385283, -0.5450993406589790, -0.0169545159116180,
      -0.5958627534635235, 0.1475241470618822,  -0.5813409052559602,
      0.0016999349688338,  0.4744261346130221,  -0.6397791626846800,
      0.2327395686450691,  -0.6716066006808359, 0.6326848319204883,
      0.5990934485500303,  -0.2410156363402478, -0.7077516757950508,
      0.3635351945204863,  0.2793041246582562,  -0.9226848841273261,
      -0.2255626972698646, 0.3900036027025244,  -0.5914547821371305,
      -0.2587544213372499, 0.8322026197550763,  0.0809525949283136,
      -0.5544186031341112, 0.1947406953655790,  0.2460365167610210,
      -0.0980713580148367, -0.9877698230089518, -0.1785803604621348,
      0.2413931163110139,  -0.3513561415864506, -0.2820343607606057,
      0.3853412436869255,  -0.1954147151119949, -0.2784604702272022,
      0.1631725286568535,  -0.0581643067283941, 0.9377092643907092,
      -0.0342879319221754, 0.6880186147380665,  -0.6441883162112154,
      0.4363125823831484,  -0.9317421699458910, -0.9854936653320749,
      0.4819767736355889,  0.2716427375448740,  -0.2226635138233914,
      -0.4703774041682058, -0.0461834279465023, 0.9538965120035416,
      -0.7064050875804320, 0.2756722737204620,  0.5464435863410304,
      -0.6497131868092225, -0.8510956529388432, 0.1207949181502335,
      0.5392434920672295,  0.8557008616846919,  -0.9845513175756098,
      -0.2447865838638490, 0.9655180006300321,  0.2851844026521584,
      0.0496434181684202,  0.8536902294544331,  -0.5160303662406824,
      0.1145403058190109,  -0.4310494965595135, 0.2046294826573649,
      -0.7688513120912757, -0.6788580444178884, -0.3256649678328609,
      -0.6790815053998789, -0.1598117465691402, -0.9447457352741000,
      0.8739130741412453,  -0.5596640617035036, -0.6201765381887243,
      0.3177667989408706,  -0.7279628021138145, -0.9404081485812070,
      0.2371807176952292,  -0.1069074337949079, -0.5873385485557310,
      0.1931483233032196,  0.5045067414620963,  -0.0009310107999667,
      0.8611440129462309,  -0.3429985861935170, -0.7264058088622851,
      -0.0592432183383149, 0.2313250596735721,  -0.6785419884148798,
      -0.2436136881611979, -0.6322564602263463, -0.4289991639419746,
      -0.7211539919350034, 0.1241247755286543,  -0.5830598349848393,
      0.7025487097136043,  -0.5605594291462010, 0.3856141351600524,
      -0.1761682412073626, 0.6349597784547771,  0.5104753338107393,
      0.1355113417988725,  -0.2359890440036447, 0.7043608436143884,
      -0.9720469263966880, -0.4922045816320892, -0.4759522885798384,
      0.2554501245775089,  -0.0474824426320231, 0.9199977266640922,
      0.7618813503933126,  0.9167609828839436,  0.9586565200478463,
      0.3451426359579348,  -0.7507388232016234, 0.3325034427831093,
      -0.2763033890537310, 0.6549453942396606,  -0.2788865525158422,
      -0.2505564666268052, 0.6853837625667534,  0.0401310368542553,
      -0.4815683185124364, 0.5601538386171352,  -0.4130255763717110,
      0.4593346251961885,  -0.1970138470915577, 0.2163727423635728,
      -0.6067220559790869, -0.7357784893566333, 0.8212067580157314,
      -0.8673870168661391, -0.6744405144151158, 0.2190008179111613,
      -0.1655284465865079, -0.0476014182337718, -0.2416843086308742,
      0.6968657455135645,  -0.2107547102681508, -0.4050234022787651,
      -0.0631657161897634, 0.9071692135553899,  -0.8620327854809786,
      -0.1458163105682473, 0.5918395501513904,  0.5858471753080197,
      -0.4697510525164419, 0.4784865650002732,  0.5769477428343242,
      -0.4592047176329377, 0.9703659151939410,  -0.9986659235012296,
      0.2724705936590730,  0.2880233987421510,  0.8201724331115658,
      -0.3980079209945298, 0.8243549541332893,  -0.2710492265873836,
      -0.8637790515273536, -0.0007890327659577, -0.9546165960404460,
      0.5730387769379841,  -0.2365745749686241, 0.3019001375978181,
      0.6813817271943583,  -0.9964961297718569, -0.4537438379127781,
      0.0943924867543535,  0.6433208561991519,  -0.9557057289826338,
      0.8361965598431558,  0.9989983064252097,  0.3658956212297195,
      -0.9359100259348703, 0.3970317071912324,  -0.5270608796808651,
      0.9838767963627388,  -0.5982881268957769, 0.7140678527479982,
      -0.8708502254519244, -0.9478083271709827, 0.6851879433438646,
      0.8722369249403605,  -0.8997497640341094, -0.0790826899189849,
      -0.0583598112726273, 0.3288496745891518,  0.7924301377601048,
      -0.8385560955071847, -0.0704767928728502, -0.7722596374501982,
      0.6525240278293580,  0.1795548721306977,  0.5831063959726377,
      0.7824253219868536,  0.9410356034085521,  0.5117384801612357,
      0.6828612867713562,  -0.8807200013331746, 0.1763080140490081,
      -0.8825425752742693, -0.9495050559898544, 0.7385783097655922,
      0.0388213727205859,  0.7571440345555494,  0.0453927307827875,
      -0.4047812295492350, 0.4405581433060368,  -0.1775790719278851,
      -0.8266146071812608, -0.7777891255327967, -0.0684450938666978,
      -0.4855500147743155, 0.3002343652693513,  0.0652184816869554,
      0.0930035168923262,  0.5373352345738402,  -0.0780844111497869,
      0.0482207918230939,  -0.8060014427030893, 0.7484387391108600,
      -0.2884612054121387, 0.3112418811088808,  0.2116790265202226,
      -0.7823273590742785, -0.3279714840273116, -0.4663620557117527,
      -0.5432313298318505, -0.5758660735462464, 0.9765599779729937,
      0.0425348151524263,  0.4920063737639040,  -0.9620387796105556,
      0.5602987784752373,  0.1994285783790870,  -0.3067815657597566,
      -0.8684114673927408, -0.0011768969336952, -0.1731141252540287,
      -0.0529959696190958, -0.1167573971268696, -0.4816869604503269,
      0.7484101091115596,  0.3649693625277060,  0.1043554712817358,
      -0.3847197883145907, -0.8237335811513702, -0.3038473545090528,
      -0.5056332266432428, -0.0942132432700951, 0.6161088228685658,
      0.2017255531228870,  -0.9318338224748941, -0.6578098291465830,
      0.1532952759568229,  0.8518416811010521,  0.5529670046817552,
      0.0380710463257814,  -0.8503388127555174, -0.3625468830634844,
      -0.7938521115312682, -0.5356875970539565, 0.1079351848327550,
      0.4254433059553755,  0.2949709226963591,  0.9627710620789434,
      -0.7069794790816053, 0.3980729454515690,  0.1315524188079544,
      0.6968454591670932,  0.1131184235319838,  0.7120796422413733,
      -0.2157426326977170, 0.0608105554569780,  0.9084597421214577,
      -0.2211126655683737, 0.8194098379033907,  -0.6080659986641610,
      -0.8987287073981007, 0.3729278929712445,  -0.9258173262476346,
      0.7468582190701767,  -0.3118603776300011, -0.1522283533481203,
      0.0653873468874082,  0.3051589133876320,  -0.2601186748182076,
      -0.2913075360840813, -0.7388680289668932, -0.5697357872721578,
      0.1408576517762652,  0.2520570796590873,  -0.9270918603181790,
      0.4804136964233441,  0.8465941915433570,  -0.3739898258266687,
      -0.3393665250316995, -0.2875028005277824, 0.7199500863840400,
      -0.5930778504735035, -0.0778346373227190, 0.5644151348515414,
      -0.1835913675067133, -0.1940565421652058, -0.0792565671836938,
      -0.5747813237571400, 0.4657926769828595,  0.4732595848952363,
      0.9115735349539162,  -0.6984458169999956, -0.0846295310064598,
      -0.0234965220181260, -0.7171888886408386, 0.2702454843874271,
      -0.5923812538212612, -0.3411746591333120, -0.5721704714304514,
      -0.2696902314029863, -0.2415118924458337, 0.3019694774808046,
      -0.7653622630874455, -0.0559877711095198, -0.3469877424828298,
      -0.6894915523913270, -0.0356730902124052, -0.8140201014863078,
      0.8603804064445961,  0.4050280283044949,  0.6657029107998438,
      0.3248756243272251,  -0.2938097505924520, 0.3632069888137350,
      -0.6166998919363240, -0.0074856831375432, 0.3537426049046690,
      -0.2349882872761990, 0.2988922650033563,  -0.1289552482875220,
      -0.0338710325391529, -0.4026557168334657, 0.4903614880042282,
      0.2671078884534526,  0.7354967107112009,  -0.0476801443662707,
      -0.8678662446667034, -0.8539474454591545, -0.0457133189347920,
      0.5156167119427160,  -0.9326483378904222, 0.1823000716304322,
      0.1107856652189130,  0.5565648338084854,  -0.0991823262976117,
      0.9936776406765366,  0.8967560797604957,  -0.6996269467566987,
      0.9269178997385525,  0.3334956351252578,  0.0357829271243364,
      0.3365587891587773,  -0.1276712366805079, 0.6753536450038433,
      0.3124173714498473,  -0.5140335439544708, 0.0548807420020012,
      -0.4010781910257750};
  gsl_block m_block = {484, m_data};
  const gsl_matrix m = {22, 22, 22, m_block.data, &m_block, 0};
  double v_out_data[22] = {
      0.5872732132667224,  0.5634235580538283,  0.2459202008229535,
      0.5434339503086938,  -0.2809075789157727, -0.5450993406589790,
      -0.5544186031341112, -0.2226635138233914, -0.6788580444178884,
      -0.2436136881611979, 0.9586565200478463,  -0.0476014182337718,
      -0.8637790515273536, 0.6851879433438646,  0.7571440345555494,
      -0.5432313298318505, 0.6161088228685658,  0.0608105554569780,
      -0.3393665250316995, 0.3019694774808046,  0.7354967107112009,
      -0.4010781910257750};
  gsl_block v_out_block = {22, v_out_data};
  gsl_vector v_out = {22, 1, v_out_block.data, &v_out_block, 0};
  gsl_matrix_diag(&m, &v_out);

  EXPECT_NEAR(0.5872732132667224, v_out.data[0], 1e-9);
  EXPECT_NEAR(0.5634235580538283, v_out.data[1], 1e-9);
  EXPECT_NEAR(0.2459202008229535, v_out.data[2], 1e-9);
  EXPECT_NEAR(0.5434339503086938, v_out.data[3], 1e-9);
  EXPECT_NEAR(-0.2809075789157727, v_out.data[4], 1e-9);
  EXPECT_NEAR(-0.5450993406589790, v_out.data[5], 1e-9);
  EXPECT_NEAR(-0.5544186031341112, v_out.data[6], 1e-9);
  EXPECT_NEAR(-0.2226635138233914, v_out.data[7], 1e-9);
  EXPECT_NEAR(-0.6788580444178884, v_out.data[8], 1e-9);
  EXPECT_NEAR(-0.2436136881611979, v_out.data[9], 1e-9);
  EXPECT_NEAR(0.9586565200478463, v_out.data[10], 1e-9);
  EXPECT_NEAR(-0.0476014182337718, v_out.data[11], 1e-9);
  EXPECT_NEAR(-0.8637790515273536, v_out.data[12], 1e-9);
  EXPECT_NEAR(0.6851879433438646, v_out.data[13], 1e-9);
  EXPECT_NEAR(0.7571440345555494, v_out.data[14], 1e-9);
  EXPECT_NEAR(-0.5432313298318505, v_out.data[15], 1e-9);
  EXPECT_NEAR(0.6161088228685658, v_out.data[16], 1e-9);
  EXPECT_NEAR(0.0608105554569780, v_out.data[17], 1e-9);
  EXPECT_NEAR(-0.3393665250316995, v_out.data[18], 1e-9);
  EXPECT_NEAR(0.3019694774808046, v_out.data[19], 1e-9);
  EXPECT_NEAR(0.7354967107112009, v_out.data[20], 1e-9);
  EXPECT_NEAR(-0.4010781910257750, v_out.data[21], 1e-9);
}

TEST(GSLVecSaturateTest, Bounded3) {
  double v0_data[22] = {
      0.5247694820795958,  0.3627049904377531,  0.3864648680737481,
      -0.1943115246212419, 0.6941547915303763,  -0.7964185835264295,
      0.5755920975432587,  -0.6752677047686471, 0.2322361376523865,
      0.4160684950289599,  -0.0427998055721142, -0.4242383459961931,
      -0.4476880870851132, -0.5710738526077128, 0.5308842969143670,
      0.2010015985762237,  0.0357841083975521,  -0.1495389002173966,
      -0.0347627107634358, 0.4015721554003109,  -0.7511518826005177,
      0.6858072448855457};
  gsl_block v0_block = {22, v0_data};
  const gsl_vector v0 = {22, 1, v0_block.data, &v0_block, 0};
  double v_out_data[22] = {
      0.2148708876411851,  0.2148708876411851,  0.2148708876411851,
      -0.1943115246212419, 0.2148708876411851,  -0.2991121712040611,
      0.2148708876411851,  -0.2991121712040611, 0.2148708876411851,
      0.2148708876411851,  -0.0427998055721142, -0.2991121712040611,
      -0.2991121712040611, -0.2991121712040611, 0.2148708876411851,
      0.2010015985762237,  0.0357841083975521,  -0.1495389002173966,
      -0.0347627107634358, 0.2148708876411851,  -0.2991121712040611,
      0.2148708876411851};
  gsl_block v_out_block = {22, v_out_data};
  gsl_vector v_out = {22, 1, v_out_block.data, &v_out_block, 0};
  gsl_vector_saturate(&v0, -0.2991121712040611, 0.2148708876411851, &v_out);

  EXPECT_NEAR(0.2148708876411851, v_out.data[0], 1e-9);
  EXPECT_NEAR(0.2148708876411851, v_out.data[1], 1e-9);
  EXPECT_NEAR(0.2148708876411851, v_out.data[2], 1e-9);
  EXPECT_NEAR(-0.1943115246212419, v_out.data[3], 1e-9);
  EXPECT_NEAR(0.2148708876411851, v_out.data[4], 1e-9);
  EXPECT_NEAR(-0.2991121712040611, v_out.data[5], 1e-9);
  EXPECT_NEAR(0.2148708876411851, v_out.data[6], 1e-9);
  EXPECT_NEAR(-0.2991121712040611, v_out.data[7], 1e-9);
  EXPECT_NEAR(0.2148708876411851, v_out.data[8], 1e-9);
  EXPECT_NEAR(0.2148708876411851, v_out.data[9], 1e-9);
  EXPECT_NEAR(-0.0427998055721142, v_out.data[10], 1e-9);
  EXPECT_NEAR(-0.2991121712040611, v_out.data[11], 1e-9);
  EXPECT_NEAR(-0.2991121712040611, v_out.data[12], 1e-9);
  EXPECT_NEAR(-0.2991121712040611, v_out.data[13], 1e-9);
  EXPECT_NEAR(0.2148708876411851, v_out.data[14], 1e-9);
  EXPECT_NEAR(0.2010015985762237, v_out.data[15], 1e-9);
  EXPECT_NEAR(0.0357841083975521, v_out.data[16], 1e-9);
  EXPECT_NEAR(-0.1495389002173966, v_out.data[17], 1e-9);
  EXPECT_NEAR(-0.0347627107634358, v_out.data[18], 1e-9);
  EXPECT_NEAR(0.2148708876411851, v_out.data[19], 1e-9);
  EXPECT_NEAR(-0.2991121712040611, v_out.data[20], 1e-9);
  EXPECT_NEAR(0.2148708876411851, v_out.data[21], 1e-9);
}

TEST(GSLVecNormBoundTest, Normal3) {
  double v0_data[22] = {
      0.2175087407976868,  -0.0739567978303353, -0.5526637499359914,
      -0.3007706404613104, -0.6677472289066111, -0.9054301746923716,
      -0.8089074236324147, -0.2908317353850434, -0.5112954159682581,
      0.9697116935008185,  0.5784451151555299,  0.4013090952502389,
      0.2540282001340546,  -0.7163818889074196, -0.7690302344806781,
      -0.0981289336214031, -0.6322148417010760, 0.1870680672946425,
      0.8544983065633378,  0.4831245676797560,  -0.4556864631471920,
      -0.3237730525048528};
  gsl_block v0_block = {22, v0_data};
  const gsl_vector v0 = {22, 1, v0_block.data, &v0_block, 0};
  double v_norm = gsl_vector_norm_bound(&v0, 0.1351340806975233);

  EXPECT_NEAR(2.6533397619907486, v_norm, 1e-9);
}

TEST(GSLVecNormBoundTest, Bounded3) {
  double v0_data[22] = {
      0.5304621010796087,  -0.6898094790158280, -0.1091004394974653,
      0.1543456989547094,  0.9651005224386344,  0.7682479310023049,
      -0.0814639196470872, 0.4292439453822356,  -0.4418555420166654,
      0.1636806507390034,  0.6493007138167206,  -0.2953881073592957,
      -0.8032227535241343, 0.6011255512581961,  -0.6679643994577207,
      0.4157278704695062,  -0.2590513679987814, 0.8269445016631487,
      0.7050199291165613,  -0.6104410604732968, -0.0484895277883166,
      0.2726552218252978};
  gsl_block v0_block = {22, v0_data};
  const gsl_vector v0 = {22, 1, v0_block.data, &v0_block, 0};
  double v_norm = gsl_vector_norm_bound(&v0, 1.8222491328276815);

  EXPECT_NEAR(2.5610914970692118, v_norm, 1e-9);
}

TEST(GSLVecDotTest, Normal3) {
  double v0_data[22] = {
      0.1368428019863432,  0.8003333656251677,  0.2727795003995599,
      0.4313647161503320,  -0.8879301337867000, 0.4158369898833310,
      -0.9172510385690660, 0.1027449661051119,  -0.5949464057546661,
      0.9707482796488287,  0.4591570221953765,  -0.4663374206697792,
      -0.1355750206327893, -0.9629321167570475, -0.8928958585345581,
      -0.6794540886175122, 0.9828146020906550,  0.3274723835381594,
      -0.4679328954475719, -0.2852245695934001, 0.8302727422429084,
      -0.8093607454787493};
  gsl_block v0_block = {22, v0_data};
  const gsl_vector v0 = {22, 1, v0_block.data, &v0_block, 0};
  double v1_data[22] = {
      0.8903321243997573,  0.3068984310716625,  0.5314649658963433,
      0.4437311302257148,  -0.1618634940117223, 0.9052970408499839,
      0.1775165714312970,  -0.3270881849243839, 0.8495616566237214,
      -0.1780832035980637, -0.3626640543003985, -0.8767094338929864,
      0.0884388403215941,  -0.2595525432989230, 0.1421659585783706,
      0.6641351721479534,  0.2627319306098281,  -0.4991300676262460,
      -0.2635917657771114, -0.0719843056382403, -0.0478365182235365,
      -0.2714741516249053};
  gsl_block v1_block = {22, v1_data};
  const gsl_vector v1 = {22, 1, v1_block.data, &v1_block, 0};
  double v_dot = gsl_vector_dot(&v0, &v1);

  EXPECT_NEAR(0.6699861041513930, v_dot, 1e-9);
}

TEST(GSLMatTraceTest, Normal4) {
  double m_data[9] = {
      0.4586099851621306,  -0.8865752926350225, 0.7442690216753509,
      0.4209657295622657,  -0.7269515151296981, -0.4596969475310975,
      -0.2757647270550745, 0.6519946521404743,  0.7927352192994752};
  gsl_block m_block = {9, m_data};
  const gsl_matrix m = {3, 3, 3, m_block.data, &m_block, 0};
  double m_trace = gsl_matrix_trace(&m);

  EXPECT_NEAR(0.5243936893319077, m_trace, 1e-9);
}

TEST(GSLMatDiagTest, Normal4) {
  double m_data[484] = {
      0.9784278391287566,  0.5252498864060990,  0.0652491269565605,
      0.6664209282895941,  0.2825938984759206,  0.7280465472989464,
      -0.1704212186295817, 0.1224206238250811,  0.3534145424606021,
      -0.3471845419465720, -0.7525814049332709, -0.4560269334460254,
      -0.9918741416390930, -0.7478887153535274, 0.5321627127539059,
      -0.3755414468585363, 0.2527786588805494,  -0.8770819726110899,
      -0.1660853778134723, 0.8320473179125483,  0.2770566952391842,
      -0.1469763079001782, 0.6788816014292600,  0.1813730087149017,
      -0.0528596021373862, 0.7424142232081723,  0.7941475380970044,
      0.6341454386265883,  0.7320931619265487,  0.9803585828330168,
      -0.1427471686797459, -0.6548036650051623, -0.9684732498716706,
      0.9768458513895546,  -0.2987463074946162, -0.1178470347189067,
      -0.2061354037324128, -0.4612147042582697, -0.8248953082745609,
      -0.9014593892880274, 0.1326087411917054,  -0.1240629056284164,
      0.7060516279911360,  0.2766904485844199,  0.9509869963025714,
      -0.2416664577992902, -0.6780890637316073, -0.3907951450314395,
      0.0586500646040922,  0.0864771606648147,  0.4047535072774728,
      -0.2306584070682389, -0.5750786976735789, -0.5461939552958339,
      0.9194888948140036,  0.8665766154285695,  -0.8024305054540468,
      -0.3953810356873690, 0.3778857329180922,  0.9664733271648900,
      0.2501409887421420,  -0.3426766947719924, 0.9528885184560159,
      -0.9643926468648234, 0.1241191269260813,  0.6403886563647754,
      -0.4036584611447411, -0.0021338112207490, 0.9221775310818925,
      0.7885441971617546,  -0.5588922524551760, 0.3231821602788527,
      -0.6894776238838209, -0.3726584109734290, 0.5449115402164066,
      -0.4492531895417964, -0.3316533576544176, 0.0266179240886326,
      -0.1205594495441764, 0.7799270986111957,  -0.7198975272929584,
      -0.2934182152382021, 0.0430486755851072,  0.0339999986477364,
      -0.1671134987213712, -0.3129730767588224, 0.5698708964575960,
      0.1847774110244784,  -0.0876474888350269, -0.1419060720880159,
      -0.6210657516115126, 0.2792181735114465,  -0.4912872676341729,
      -0.7802682876983988, 0.0597238757186123,  0.9190823490447608,
      0.0429224512803155,  0.2767202489295717,  -0.8890760561276840,
      -0.9231379300694167, 0.0928284812277063,  -0.1389198851734386,
      -0.9976681412866886, 0.2965395214203603,  -0.7844466061599429,
      0.6713551856830640,  0.8203263225908590,  -0.1654631931194057,
      0.3326456969003151,  -0.9720439053818959, 0.4206192831757583,
      0.2884580038808138,  -0.6784462266000408, -0.4297523624820963,
      -0.6024813993881919, 0.4846744929682756,  0.2588874509903027,
      -0.4459350520177057, 0.4049440499675494,  -0.3604551233148221,
      0.9238602718572564,  0.6410537466428037,  -0.8657345729818782,
      0.0183607453423640,  -0.6471963973069785, 0.9019140552703784,
      -0.9174211807055481, -0.2004902332263561, 0.8556764110504829,
      -0.7461873823031120, -0.8248991082727837, 0.1564175171363360,
      -0.7946017449385161, 0.7200028050460705,  0.5520780854928780,
      0.6808279376168589,  0.7677423474983396,  -0.3763884838439682,
      -0.2212438603569138, 0.2685770297150953,  -0.8387320901603343,
      0.1877793550208409,  -0.7482304508853617, 0.9598921701550136,
      -0.6496844142586093, 0.4736597944269154,  -0.0655886025527481,
      0.7877936117972830,  0.6449189983302495,  -0.0108237606233570,
      -0.0387675597435506, 0.3563245745554680,  -0.0942045642665317,
      -0.5928003064979888, -0.9486884566803144, -0.8119265808553060,
      0.8800666470102421,  0.9057926468579076,  -0.4910582556577490,
      -0.8046738427352718, -0.1871148783256109, -0.5972656647014487,
      -0.2137649827189156, -0.9474346559170965, 0.5438796985423238,
      0.8864553307705485,  0.5053672921190404,  -0.8181776406249754,
      0.8841956367463399,  0.1843943327675936,  -0.1658211898448192,
      0.5825675625050926,  0.1815056288682759,  0.8175642451252616,
      -0.2699596498230421, -0.7251831432396201, 0.9709561106082412,
      0.7025158788857171,  -0.4217368466045208, 0.9548642421706048,
      -0.8625351633916969, -0.5897108742522537, -0.5403641291259373,
      0.1090544514979703,  0.6450378930543919,  -0.0514573519514678,
      0.5865971208587231,  0.6898001672393737,  -0.2618308889686769,
      -0.9345202712468097, 0.2856267077046661,  0.5486887739222852,
      -0.6848053645022232, -0.8669788771792855, -0.3127245447356586,
      0.0722429035169696,  0.7038950808289186,  0.0161593596423752,
      0.4020680257994240,  0.2926818956528310,  0.5289869631879753,
      -0.9311601231998947, 0.2444896010435003,  0.5652135644115697,
      0.4935959450842062,  -0.8594606820781907, 0.6338135310648279,
      -0.5562342385702255, 0.8680277270131489,  -0.2381211071447504,
      -0.5102184259166800, -0.0654063777138378, 0.6761341634602784,
      -0.7853666217593787, 0.1608794753876790,  -0.5858208792333235,
      0.7849231166076256,  -0.6081394045295416, 0.5808260832296135,
      0.5658506405701715,  -0.7214214090143352, 0.1249657708787550,
      0.3931811183993161,  -0.1134732107762786, 0.0019649974538074,
      -0.8811043918369497, 0.0076737477850166,  -0.6737261786755928,
      -0.3643818657675117, 0.0510585142553341,  -0.2641708510327088,
      -0.5285119570473282, -0.7668020703552285, 0.2476567871013642,
      -0.0335135439073373, 0.9729548899316138,  0.2196861621185666,
      -0.4859082502971162, -0.1104272622036997, 0.8187346011155743,
      0.5922695614036191,  -0.7104761965558826, -0.9051843135795661,
      -0.5383561011731115, 0.1745774588574460,  -0.4147939013030804,
      -0.6291983831135386, 0.4282109249146440,  0.3721838632660948,
      -0.8291975736966524, -0.4675496061140658, -0.4540218616617393,
      0.6697566721115267,  0.1144342355423236,  0.5950717984227321,
      0.4108984023831996,  0.0327395822997953,  -0.9397656916764190,
      -0.3407152004990590, 0.6973811113183586,  0.3972781445116349,
      0.8865089251318923,  -0.1694815147788191, -0.1874458060054163,
      -0.2649455892029720, -0.3502375666359696, -0.4789189605836439,
      -0.9196617312731379, 0.0086895249965568,  0.7002984581165981,
      -0.0265744980233060, -0.9916141760390931, 0.8896071335243001,
      0.8854737645599071,  -0.1765467183086118, -0.4323615303992761,
      0.8170065128233037,  -0.6787964430657287, -0.1822614059273344,
      -0.8410895761494124, -0.5909009864942676, -0.1629617765078992,
      -0.9622025463633064, -0.0359360079990867, 0.9316956534433496,
      0.2609798151088987,  -0.6379837792069474, 0.8936220709991969,
      -0.3231288713728806, 0.8593215612702991,  0.9680593806063511,
      0.4033009691862273,  -0.5871009294935352, -0.9088929248617676,
      0.7068078550134971,  -0.4719360368310523, -0.2511082941631724,
      -0.0400843684901320, 0.5007918290717173,  -0.1295027057813807,
      0.2505104802444087,  0.2413892891630693,  0.1321380800735144,
      0.8825193230807422,  -0.6758940582973296, 0.3579304671439529,
      0.1787929162952937,  -0.8520759883002875, -0.7199681842515504,
      -0.2002128136230463, -0.3491798219194622, 0.2513739217417625,
      -0.2326047169382162, 0.4205964123909083,  -0.1772135073075627,
      0.8462286243623940,  0.1219632138038713,  0.5189487869013665,
      0.0040552510146030,  0.4844307506038508,  -0.2345316763292269,
      -0.4950111624422584, 0.7839455935150366,  0.4644548571308282,
      -0.6547446970259052, -0.2536083531115996, 0.1013962848691037,
      -0.5964756641971727, 0.0599260293110035,  0.0970432591201233,
      0.5237265799925432,  -0.8721666187289725, -0.0699896296867637,
      0.4494735047876171,  0.0949782938659669,  0.5326506675096225,
      0.6435492152831175,  0.7776053592839429,  0.9032817378121620,
      0.1741943596699824,  0.8483103760280351,  0.3688799688998983,
      -0.2998457334636224, -0.8046244950470780, -0.6497765504807351,
      0.3831640362965545,  0.0233052295587126,  0.2161294620563727,
      0.6435544678024805,  -0.7504192593688770, 0.2384600139271429,
      0.8304779456499265,  -0.6210889954216021, -0.4096513144499090,
      -0.3624443315549337, 0.0417421489328194,  0.0720012801180181,
      0.9897963214492465,  0.0681844947080261,  -0.4564502515217341,
      0.3782085651048064,  -0.1430198305477499, -0.3843835012228567,
      0.0394046842096127,  0.0060552112120267,  -0.7480821048132476,
      -0.9200616507627322, 0.8914159678695670,  -0.9027808751020623,
      -0.0719124265990883, -0.7077476652723209, -0.2231799318082210,
      0.1467027610475362,  -0.6511301867613077, 0.5590093578713682,
      0.9705905758065150,  -0.5161647735102293, -0.1557105750646943,
      -0.5772345762569813, -0.7616299074021029, 0.1325187740547575,
      0.3252263120342866,  -0.5847033514688194, 0.6243977848525439,
      0.3641944243720379,  0.6315309370959699,  -0.5998353185492451,
      -0.8729033898191305, 0.9797020291988592,  -0.6196579698870419,
      -0.5893515021246933, 0.9129689758988406,  0.8355935383734168,
      0.4288673217622898,  -0.4714413318389643, 0.7865124802218066,
      0.8467842172409885,  -0.3419050141575986, 0.1052304855087800,
      -0.4865300439716471, 0.6568656482367596,  0.3500997304694164,
      -0.0787651596086041, 0.4658896781648250,  -0.2855549466439409,
      -0.2706181833895942, -0.4799900199794480, 0.8455527257815685,
      -0.4595470834117148, 0.6132352956386586,  0.9738374371988003,
      0.9069050385014699,  -0.5885026320365090, -0.3231470888542989,
      0.9520144195793818,  0.3846501901780108,  -0.7572528793440667,
      0.5713707031423259,  0.8845973106395304,  0.5334135186363187,
      -0.5200003641686963, 0.4484487523837779,  0.2683183104619622,
      -0.4431497269046669, -0.7068522427652399, 0.1657199039440063,
      -0.4770770213585669, 0.0491708787637539,  -0.4650886835926904,
      0.2115377681071033,  -0.3895957042404892, 0.2729791268142645,
      -0.7104357852652241, -0.1797336486489112, 0.8513779129069139,
      -0.9994021608463159, 0.8931676763221625,  -0.5453734705666557,
      -0.0970874399095036, -0.5767817461730829, 0.7652169079794515,
      -0.0753005963576683, 0.8997028239803970,  -0.1269118552697157,
      0.9927543959658145,  -0.3119298818553209, 0.7983238284999468,
      -0.9209399216018423, -0.8537495674949189, -0.8114816262560971,
      0.3828791407047565,  -0.4305315740127451, 0.9360891428559324,
      0.8801525890457362,  -0.9711007986068338, 0.0929918041636022,
      -0.9152322502363011, 0.5243770748557992,  0.6457964869564676,
      -0.4265788713568637, 0.4009023052419520,  0.8693297264443576,
      -0.4414496175746889, 0.2212853286056835,  -0.0077237078979180,
      0.0511995134301260,  0.4175301656067067,  -0.4027397124471952,
      -0.1736934832384980, 0.6495250760868529,  0.5660309319650281,
      0.7101920830834854,  -0.8380737434268328, -0.9366057558936502,
      0.2276640769504239,  0.5071259068151481,  -0.5098675665911725,
      -0.2859161318976582, -0.2486784827592976, -0.1191911181865262,
      0.9256736792993325,  -0.2377540209680176, 0.3314888769839504,
      0.9610149363578462};
  gsl_block m_block = {484, m_data};
  const gsl_matrix m = {22, 22, 22, m_block.data, &m_block, 0};
  double v_out_data[22] = {
      0.9784278391287566,  0.1813730087149017,  -0.6780890637316073,
      0.7885441971617546,  -0.4912872676341729, 0.4846744929682756,
      -0.2212438603569138, -0.5972656647014487, 0.6450378930543919,
      -0.5562342385702255, -0.2641708510327088, 0.1144342355423236,
      0.8170065128233037,  -0.1295027057813807, 0.7839455935150366,
      0.3831640362965545,  -0.9027808751020623, 0.9129689758988406,
      0.9520144195793818,  -0.5453734705666557, 0.4009023052419520,
      0.9610149363578462};
  gsl_block v_out_block = {22, v_out_data};
  gsl_vector v_out = {22, 1, v_out_block.data, &v_out_block, 0};
  gsl_matrix_diag(&m, &v_out);

  EXPECT_NEAR(0.9784278391287566, v_out.data[0], 1e-9);
  EXPECT_NEAR(0.1813730087149017, v_out.data[1], 1e-9);
  EXPECT_NEAR(-0.6780890637316073, v_out.data[2], 1e-9);
  EXPECT_NEAR(0.7885441971617546, v_out.data[3], 1e-9);
  EXPECT_NEAR(-0.4912872676341729, v_out.data[4], 1e-9);
  EXPECT_NEAR(0.4846744929682756, v_out.data[5], 1e-9);
  EXPECT_NEAR(-0.2212438603569138, v_out.data[6], 1e-9);
  EXPECT_NEAR(-0.5972656647014487, v_out.data[7], 1e-9);
  EXPECT_NEAR(0.6450378930543919, v_out.data[8], 1e-9);
  EXPECT_NEAR(-0.5562342385702255, v_out.data[9], 1e-9);
  EXPECT_NEAR(-0.2641708510327088, v_out.data[10], 1e-9);
  EXPECT_NEAR(0.1144342355423236, v_out.data[11], 1e-9);
  EXPECT_NEAR(0.8170065128233037, v_out.data[12], 1e-9);
  EXPECT_NEAR(-0.1295027057813807, v_out.data[13], 1e-9);
  EXPECT_NEAR(0.7839455935150366, v_out.data[14], 1e-9);
  EXPECT_NEAR(0.3831640362965545, v_out.data[15], 1e-9);
  EXPECT_NEAR(-0.9027808751020623, v_out.data[16], 1e-9);
  EXPECT_NEAR(0.9129689758988406, v_out.data[17], 1e-9);
  EXPECT_NEAR(0.9520144195793818, v_out.data[18], 1e-9);
  EXPECT_NEAR(-0.5453734705666557, v_out.data[19], 1e-9);
  EXPECT_NEAR(0.4009023052419520, v_out.data[20], 1e-9);
  EXPECT_NEAR(0.9610149363578462, v_out.data[21], 1e-9);
}

TEST(GSLVecSaturateTest, Bounded4) {
  double v0_data[22] = {
      0.6551867685579851,  -0.4887371184993261, 0.4517688907210851,
      0.5059698789610241,  -0.1800884707233683, 0.4341860076266801,
      0.7087599202489370,  0.5754014248614572,  -0.6465311596291174,
      -0.4995093173608245, -0.2283674493043861, -0.9014328921117525,
      -0.6363353672373084, -0.0143925514381493, 0.3383132223497405,
      -0.6742920168150697, 0.9402371365747340,  0.2139969018476338,
      0.2552918238895079,  0.6365223438594392,  0.1843987218984677,
      0.6550471116645695};
  gsl_block v0_block = {22, v0_data};
  const gsl_vector v0 = {22, 1, v0_block.data, &v0_block, 0};
  double v_out_data[22] = {
      0.6551867685579851,  -0.4887371184993261, 0.4517688907210851,
      0.5059698789610241,  -0.1800884707233683, 0.4341860076266801,
      0.7087599202489370,  0.5754014248614572,  -0.5091268747219766,
      -0.4995093173608245, -0.2283674493043861, -0.5091268747219766,
      -0.5091268747219766, -0.0143925514381493, 0.3383132223497405,
      -0.5091268747219766, 0.8820218351465129,  0.2139969018476338,
      0.2552918238895079,  0.6365223438594392,  0.1843987218984677,
      0.6550471116645695};
  gsl_block v_out_block = {22, v_out_data};
  gsl_vector v_out = {22, 1, v_out_block.data, &v_out_block, 0};
  gsl_vector_saturate(&v0, -0.5091268747219766, 0.8820218351465129, &v_out);

  EXPECT_NEAR(0.6551867685579851, v_out.data[0], 1e-9);
  EXPECT_NEAR(-0.4887371184993261, v_out.data[1], 1e-9);
  EXPECT_NEAR(0.4517688907210851, v_out.data[2], 1e-9);
  EXPECT_NEAR(0.5059698789610241, v_out.data[3], 1e-9);
  EXPECT_NEAR(-0.1800884707233683, v_out.data[4], 1e-9);
  EXPECT_NEAR(0.4341860076266801, v_out.data[5], 1e-9);
  EXPECT_NEAR(0.7087599202489370, v_out.data[6], 1e-9);
  EXPECT_NEAR(0.5754014248614572, v_out.data[7], 1e-9);
  EXPECT_NEAR(-0.5091268747219766, v_out.data[8], 1e-9);
  EXPECT_NEAR(-0.4995093173608245, v_out.data[9], 1e-9);
  EXPECT_NEAR(-0.2283674493043861, v_out.data[10], 1e-9);
  EXPECT_NEAR(-0.5091268747219766, v_out.data[11], 1e-9);
  EXPECT_NEAR(-0.5091268747219766, v_out.data[12], 1e-9);
  EXPECT_NEAR(-0.0143925514381493, v_out.data[13], 1e-9);
  EXPECT_NEAR(0.3383132223497405, v_out.data[14], 1e-9);
  EXPECT_NEAR(-0.5091268747219766, v_out.data[15], 1e-9);
  EXPECT_NEAR(0.8820218351465129, v_out.data[16], 1e-9);
  EXPECT_NEAR(0.2139969018476338, v_out.data[17], 1e-9);
  EXPECT_NEAR(0.2552918238895079, v_out.data[18], 1e-9);
  EXPECT_NEAR(0.6365223438594392, v_out.data[19], 1e-9);
  EXPECT_NEAR(0.1843987218984677, v_out.data[20], 1e-9);
  EXPECT_NEAR(0.6550471116645695, v_out.data[21], 1e-9);
}

TEST(GSLVecNormBoundTest, Normal4) {
  double v0_data[22] = {
      -0.0601914771464085, -0.4320440353170660, -0.9138865362858917,
      -0.7334554478756057, 0.7317667274237456,  0.6719751290823228,
      -0.7093069530347784, -0.7307597956787206, 0.2721656980963250,
      0.7439043767825189,  -0.6606313034653339, 0.3030186261976140,
      -0.6539574477087946, -0.8155949803008553, -0.9834181291663000,
      0.2409230887886793,  -0.4666921731161049, 0.5403502815697727,
      0.4690281493074104,  0.8982080003084418,  -0.8134148512138326,
      -0.2085083380971571};
  gsl_block v0_block = {22, v0_data};
  const gsl_vector v0 = {22, 1, v0_block.data, &v0_block, 0};
  double v_norm = gsl_vector_norm_bound(&v0, 0.9986016251668852);

  EXPECT_NEAR(3.0171156665846883, v_norm, 1e-9);
}

TEST(GSLVecNormBoundTest, Bounded4) {
  double v0_data[22] = {
      0.2476157835112773,  -0.3982153847348187, -0.9322421131796628,
      0.6479840620617590,  0.6176037319138981,  -0.9824204181640221,
      0.6700402073758693,  0.9038076294910025,  -0.6015116040562272,
      -0.4133818335289658, 0.1602431564312929,  0.8822212476536690,
      0.6521965665060454,  0.7031864933873857,  -0.0304688014037977,
      0.9672467853917699,  -0.9542017834426930, -0.1200380966044312,
      0.3347991364309135,  -0.0837404971985487, -0.1106725260073500,
      0.0355485653281771};
  gsl_block v0_block = {22, v0_data};
  const gsl_vector v0 = {22, 1, v0_block.data, &v0_block, 0};
  double v_norm = gsl_vector_norm_bound(&v0, 2.6154500402642795);

  EXPECT_NEAR(2.8934060560926449, v_norm, 1e-9);
}

TEST(GSLVecDotTest, Normal4) {
  double v0_data[22] = {
      -0.5421331123122870, -0.6142917756960671, 0.5586953819849378,
      -0.9582348065187396, 0.2354626588920281,  0.3032154206773008,
      -0.0101412335176143, -0.9007207504971211, -0.7979053061820951,
      -0.0970373772961151, 0.0519379712133319,  -0.7325050177218662,
      0.7288109672453140,  -0.2173192279709053, 0.6910627850135345,
      0.7991840236234076,  -0.0497928428304739, -0.2988214351863745,
      -0.6421990254303032, 0.1427710616056848,  0.5100978677437851,
      0.6797786034769684};
  gsl_block v0_block = {22, v0_data};
  const gsl_vector v0 = {22, 1, v0_block.data, &v0_block, 0};
  double v1_data[22] = {
      0.3501115689673930,  0.4834460615786105,  -0.3228839090677089,
      -0.4193910451079625, 0.4486653669384042,  -0.1059987083001996,
      0.8195429007618877,  -0.6389255217063103, -0.4281747764929216,
      -0.8645652836161830, -0.1869386613818893, -0.2886201108461857,
      0.2191786330697991,  0.1138783800743131,  0.0271704117208653,
      -0.1414589017694632, -0.2324583614197284, -0.5242812256242975,
      0.6017726009091591,  -0.8944785337904315, -0.0511254631464524,
      -0.8624458567474689};
  gsl_block v1_block = {22, v1_data};
  const gsl_vector v1 = {22, 1, v1_block.data, &v1_block, 0};
  double v_dot = gsl_vector_dot(&v0, &v1);

  EXPECT_NEAR(0.0850708385238701, v_dot, 1e-9);
}

int main(int argc, char **argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}
